An Adaptive Scheme for Robot Localization and Mapping with Dynamically Configurable Inter-Beacon Range Measurements

Sensors 2014,14, 7684-7710; doi:10.3390/s140507684 OPEN ACCESS sensors ISSN 1424-8220 www.mdpi.com/journal/sensors Article An Adaptive Scheme for Robot Localization and Mapping with Dynamically Configurable Inter-Beacon Range Measurements Arturo Torres-Gonz´ alez, Jose Ramiro Martinez-de Dios * and Anibal Ollero Robotics Vision and Control Group, University of Sevilla, Escuela Superior de Ingenieros, c/Camino de los Descubrimientos s/n, 41092 Seville, Spain; E-Mails: [email protected] (A.T.-G.); [email protected] (A.O.) *Author to whom correspondence should be addressed; E-Mail: [email protected]; Tel.: +34-954-487-357; Fax: +34-954-487-340. Received: 31 December 2013; in revised form: 10 April 2014 / Accepted: 11 April 2014 / Published: 25 April 2014 Abstract: This work is motivated by robot-sensor network cooperation techniques where sensor nodes (beacons) are used as landmarks for range-only (RO) simultaneous localization and mapping (SLAM). This paper presents a RO-SLAM scheme that actuates over the measurement gathering process using mechanisms that dynamically modify the rate and variety of measurements that are integrated in the SLAM filter. It includes a measurement gathering module that can be configured to collect direct robot-beacon and inter-beacon measurements with different inter-beacon depth levels and at different rates. It also includes a supervision module that monitors the SLAM performance and dynamically selects the measurement gathering configuration balancing SLAM accuracy and resource consumption. The proposed scheme has been applied to an extended Kalman filter SLAM with auxiliary particle filters for beacon initialization (PF-EKF SLAM) and validated with experiments performed in the CONET Integrated Testbed. It achieved lower map and robot errors (34% and 14%, respectively) than traditional methods with a lower computational burden (16%) and similar beacon energy consumption. Keywords: robot-sensor network cooperation; range-only SLAM; sensor networks Sensors 2014,14 7685 1. Introduction This paper deals with range only SLAM, in which a robot integrates range measurements of a set of static landmarks in order to build a local map from an unknown environment and to simultaneously self-locate in that map. RO-SLAM can be very useful in a wide range of applications, such as advanced monitoring or search and rescue, in which robots cooperate with sensor nodes endowed with sensing, computing and communication capabilities. This interest has motivated the development of a growing variety of RO-SLAM methods for sensor networks [1–4], in which sensor nodes (beacons) are used as RO-SLAM landmarks. However, the great majority of these techniques consider beacons as passive landmarks, disregarding many of their capabilities. In the last few years, several SLAM methods have been combined with active perception tools. These mechanisms evaluate different sensing actions in order to dynamically choose the action that maximizes the information acquired by the robot. Active perception tools have been used in SLAM to proactively decide on the robot motions in order to explore the environment and, hence, reduce mapping uncertainty [6–9]. The great majority of these schemes have been developed for visual SLAM and actuate only over the robot motion and/or camera orientation. This paper presents an adaptive RO-SLAM scheme that actuates over the measurement gathering process and includes mechanisms that intentionally modify the rate and variety of range measurements that are integrated in the SLAM filter. It exploits the fact that in most cases, beacons in RO-SLAM are implemented by nodes of a sensor network, which can execute flexible ad hoc measurement gathering strategies. Besides the SLAM filter, which integrates all measurements gathered, the proposed scheme is based on another two main modules: the measurement gathering module and the SLAM supervisor. The measurement gathering module enables taking and collecting direct robot-beacon, as well as measurements between beacons, called inter-beacon. It performs a controlled flooding protocol in which each beacon takes range measurements to its nearby beacons and transmits them back to the robot, naturally avoiding repeated measurements. Measurement gathering can be configured to be performed at different rates and also with different inter-beacon depth levels, so that the robot can integrate measurements of beacons that are distant from the robot’s sensing zone. Integration in SLAM of inter-beacon measurements significantly improves map estimation. Furthermore, higher gathering rates result in more accurate robot location estimation. However, both involve higher consumption of resources, such as the energy required to take and transmit measurements, the bandwidth required by the measurement collection protocol and the computational burden necessary to integrate the larger number of measurements. To cope with this, the proposed scheme includes a supervision module that monitors the SLAM performance and dynamically selects the measurement gathering settings, balancing accuracy and resource consumption. The proposed scheme exploits the capability of the beacons of taking range measurements to other beacons and transmitting them using ad hoc protocols. It integrates in SLAM not only direct measurements, as traditional methods, but also inter-beacon measurements with different depth levels. Besides, it benefits from the flexibility of modifying the measurement gathering rate and inter-beacon depth level, allowing adaptation to different conditions. Sensors 2014,14 7686 The proposed scheme and its modules are general and can be applied to any SLAM filter. In this paper, it has been applied to a PF-EKF SLAM filter (described in Annex A), which is probably one of the best known and most widely researched methods. It has been validated in experiments performed in the CONET (Cooperating Objects Network of Excellence) Integrated Testbed (http://conet.us.es) [10]. In these experiments, it achieved significantly lower map and robot errors than the traditional RO-SLAM schemes with lower computational burden and similar beacon energy consumption. Besides, it showed better robustness against measurement and odometry noise levels. The paper is organized as follows. Section 2briefly summarizes the related work. Section 3presents the proposed scheme and motivates it with potential applications. Section 4describes the collection of inter-beacon measurements and their integration in the SLAM filter. Section 5presents the SLAM supervision module. Section 6experimentally evaluates the impact of the measurement gathering rate and inter-beacon depth level on SLAM performance and selects the parameters of the supervision module used in the experiments. Its evaluation and comparison with other schemes are in Section 7. Finally, conclusions are in Section 8. 2. Related Work Range only SLAM methods rely only on range measurements. Different devices have been used to provide this kind of information, e.g., wireless sensor network (WSN) nodes [1–3] or RFID units [11,12]. Range measurements have the problem of partial observability (also present in bearing sensors): only one measurement is insufficient to constrain one location. Thus, RO-SLAM methods require the robot to move and integrate measurements from different positions in order to initialize the landmark locations. Two basic approaches have been used for landmark initialization: directly introducing the measurements using a multi-hypothesis SLAM filter (undelayed) or combining the SLAM filter with specific landmark initialization tools (delayed). The extended Kalman filter (EKF) is probably the most commonly adopted SLAM filter. It has been combined with landmark initialization tools, such as particle filters [13], probability grids [14,15] and trilateration [2]. Trilateration methods, although simple and efficient, are very (too) sensitive to measurement noise. It was the first approach, but was soon discarded, since the SLAM performance is very dependent on the accuracy in beacon initialization. Probability grids provide better initialization, but their accuracy depends on the size and resolution of the grid. Particle filters (PFs) are maybe the most widely used landmark initialization tools in RO-SLAM. They provide better accuracy and a good number of mechanisms have been developed to reduce their computational burden. PFs have also been combined with Rao–Blackwellised particle filters (RBPF) in works, such as [16,17]. Undelayed SLAM schemes address the multi-hypothesis problem without requiring specific initialization tools. Works in [18,19] combined EKF with methods based on the sum of Gaussians. The greater majority of RO-SLAM methods use only direct measurements between the robot and static beacons, disregarding the fact that most beacons can organize into sensor networks, can communicate with other beacons and can compute range measurements to other beacons. This is the case of using sensor network nodes as beacons. In recent years, the interest in schemes where robots Sensors 2014,14 7687 cooperate with sensor networks has motivated the development of a number of RO-SLAM methods for sensor networks [1–3], in which sensor nodes (beacons) are used as landmarks. Integrating in SLAM inter-beacon range measurements, i.e., measurements between different beacons, involves a number of interesting advantages: they reduce map and robot estimation errors and accelerate beacon initialization. These advantages are highlighted in previous works from the authors, such as [19]. However, despite these advantages, very few methods exploiting inter-beacon measurements have been proposed. The general idea of using inter-beacon measurements was given in [14], in which different ways for incorporating inter-beacon measurements were proposed, mainly by using virtual nodes and adopting off-line map improvement using multidimensional scaling. These off-line approaches are not suitable for most applications, which require on-line map and robot locations. In the last few years, several SLAM methods have been combined with active perception tools. These mechanisms evaluate different sensing actions that the robot can take in order to dynamically select the action that maximizes the information acquired by the robot. Active perception tools could be used in SLAM in order to accelerate the convergence of the uncertainty of an algorithm, to improve estimation accuracies or to increase the area explored by the robot. Almost all active SLAM works have been focused on visual SLAM. Davison et al. [6] published one of the first general active vision systems for autonomous localization, addressing issues such as uncertainty-based measurement selection, automatic map-maintenance and goal-directed steering. Vidal-Calleja et al. [7] actuated by moving the robot and the camera in order to optimize both localization and mapping uncertainties. Frintrop and Jensfelt [8] used an active camera to track landmarks and explore unknown environments in visual SLAM. A very low number of active RO-SLAM methods have been reported. Merino et al. [20] developed a method that actuated over the steering of a robot in order to maximize uncertainty reduction in sum of Gaussians beacon initialization tools. Furthermore, almost all active SLAM schemes actuate over the robot trajectory. In fact, the term ‘active SLAM’ [21] is usually used to refer to the integration of trajectory planning in SLAM. This paper presents a RO-SLAM scheme that actuates by configuring the measurement rate and inter-beacon depth level of the measurement gathering module. To the best of our knowledge, it is the first reported RO-SLAM method that actuates over the measurement gathering process, integrating inter-beacon measurements. 3. General Overview Our work is motivated by schemes of cooperation between robots and sensor networks. Consider a GPS-denied environment, where a large number of sensor nodes have been deployed at unknown locations. For instance, they have been randomly thrown for monitoring a disaster or an accident in an industrial or urban area, where preexisting infrastructure can be damaged. The basic role of each sensor node is to periodically take measurements (e.g., toxic gas concentration), filter and compute statistics of the measurements and transmit them to a base station. For these tasks, the nodes must be endowed with sensing, communication and computational capabilities. In fact, this is the case of most commercial off-the-shelf (COTS) sensor nodes. Assume that each node is equipped with a range sensor and can measure the distance to the robot or to other nodes within its sensing zone. Again, this is not a constraint. Sensors 2014,14 7688 For instance, most COTS nodes can measure the received signal strength (RSS) from incoming messages and can measure the range to the emitting node [22]. From now on, in the paper, the terms beacons and nodes will be used synonymously. The application is to monitor the status of the event at the base station [23]. Having accurate estimations of the locations of the robot and of each node is necessary for accurate event monitoring and also enables advanced robot-sensor network cooperation strategies of interest in these problems, such as using the robot for sensor node deployment [24], replacement [25,26] or collecting data from the sensors [27], among others. GPS cannot be used. A RO-SLAM method using sensor nodes (beacons) as landmarks can be very useful for on-line estimating of node and robot locations. The robot initial pose can be used to relate this local map to global coordinates. RO-SLAM can have advantages w.r.t. visual SLAM in this problem, where suitable lighting conditions are not granted. Besides, using beacons as landmarks naturally solves data association. Using sensor network nodes as landmarks can be very useful in SLAM. Nodes can measure the range to each other and collect the measurements using ad hoc protocols. Integrating inter-beacon measurements significantly improves the map estimation and, indirectly, the robot estimation [19]. The proposed scheme benefits from inter-beacon measurements in a dynamic and adaptive way. It employs self-adaptation mechanisms to dynamically modify the number and variety of range measurements that are integrated in the SLAM filter; see Figure 1. Figure 1. General picture of the proposed scheme. The measurement gathering module is responsible for taking and collecting direct robot-beacon and also inter-beacon measurements. When a measurement gathering event is triggered, the robot starts a controlled flooding protocol in which each beacon takes range measurements to its neighbor beacons and transmits the measurements back to the robot. Measurement gathering can be configured with different inter-beacon depth levels, so that it can collect measurements of beacons that are distant from the robot’s sensing zone. High inter-beacon depth levels allow for collecting more measurements and of more distant beacons than with low-depth levels. The higher the inter-beacon depth level, the higher the amount of information that can be integrated in SLAM, reducing uncertainty. In contrast, a higher inter-beacon depth level involves higher resource consumption, such as the energy used by beacons in measuring and communication, the bandwidth required for transmitting measurements and the computational burden needed to integrate measurements in the SLAM filter. Furthermore, measurement gathering can be configured to be performed at different rates involving a similar trade-off between uncertainty reduction and resource consumption. Sensors 2014,14 7689 The measurement gathering module can be configured in different modes, each of them defined by the measurement gathering rate and the inter-beacon depth level. The objective of the SLAM supervisor is to analyze the SLAM performance and select the measurement gathering mode most suitable for the current conditions. 4. Gathering and Integration of Inter-Beacon Measurements This section is divided in two parts. The first one presents the gathering and collection of inter-beacon measurements. The second describes how inter-beacon measurements are integrated in the SLAM filter. 4.1. Measurement Gathering Measurement gathering events are triggered by the robot and affect the static beacons in two steps: the forward stage and the backward stage. In the forward stage, static beacons perform a cascade-like measuring of range distances. The origin of the cascade is the robot, and the inter-beacon depth level (IDL), the number of hops of inter-beacon measurements. The backward stage orderly collects the measurements taken by all the involved beacons. All messages interchanged include a sequence number, Seq, that identifies the measurement gathering event. Each beacon tracks the Seq of the last measurement event in which it was involved. Furthermore, all messages in the forward stage include a time to live field (ttl), which represents the number of hops remaining until the end of the forward stage. The forward stage starts when the robot broadcasts a forward message with ttl = IDL. Each beacon, i, receiving the message checks if it is a new measurement event. If it is not, the message is ignored. Otherwise, beacon iupdates ttl = ttl −1, measures its distance to all the beacons in its sensing zone and buffers the measurements in msi, the measurement set for beacon i. If field ttl >0, the beacon broadcasts a forward message with the new ttl. Each beacon keeps the identifier (ID) of its parent beacon from which it received the forward message. If ttl = 0, the forward message has reached its hop limit, and it is not retransmitted: the forward stage ends, and the backward stage starts. During the backward stage, each beacon updates its own measurement set. When beacon ireceives a backward message with suitable Seq, it adds the measurements contained in the message to its msi. When the timeout of beacon iexpires, it sends a backward message containing msito its parent beacon. Timeouts are set to allow orderly measurement collection. Figure 2illustrates the operation of measurement gathering with IDL = 0, IDL = 1 and IDL = 2 and shows the measurements collected by each beacon in each case. With IDL = 0—the traditional approach where only direct robot-beacon measurements are gathered—the robot does not transmit the forward message and only collects robot-node measurements {zr,1,zr,6}, being zr,ithe range between the robot and beacon i. With IDL = 1, the robot broadcasts a forward message with ttl = 1. Beacon ID = 1 receives the message, updates ttl = 0 and measures distances z1,j∀j∈SZ1,SZ1being the sensing zone of beacon ID = 1. Since ttl = 0, it sends a backward message with ms1to the robot. At the end of the backward stage, the robot has collected the following measurements: {zr,1,zr,6,z1,2,z1,3,z1,r,z6,r}. Notice that zi,jand zj,iare not repeated measurements; they are different measurements of the same quantity. Besides, zi,jwas taken by node iand zj,iby node j. The protocol obtains two measurements for each inter-beacon distance, except for the deepest beacons, of which only one is collected. Figure 2(right) Sensors 2014,14 7690 shows the operation of measurement gathering with IDL = 2: the robot collects measurements between beacons that are far beyond its sensing zone. In this example, beacon ID = 1 received three forward messages. The first one was sent by the robot. It also received one from beacon ID = 2 and one from ID = 3, but they were ignored, since they had non-new Seq values. Figure 2. Examples of measurement gathering with: IDL = 0 (left); IDL =1(center); and IDL =2(right). Lines represent the interchange of forward (and backward) messages. Grey circles represent sensing zones of the deepest beacons for each case. The proposed protocol prevents flooding cycles, naturally avoiding repeated measurements and canceling the need for additional filtering mechanisms. Furthermore, it avoids unnecessary measurements, message transmissions and delays, improving efficiency. 4.2. Integration of Inter-Beacon Measurements This section describes how inter-beacon measurements are integrated in SLAM. The adopted approach is general and can be applied to any SLAM filter in which inter-beacon measurements are integrated in a delayed way. We assume that each beacon can be in two stages: the beacon initialization stage and the core SLAM filter stage. In the first stage, the measurements are used to compute the beacon initial location using specific mechanisms, such as particle filters or occupancy grids, among others. Once the beacon has been initialized, its measurements are used to update the core SLAM filter. Measurement gathering with IDL >0 collects direct robot-beacon and also inter-beacon range measurements. The integration of measurement zr,i, between the robot and beacon i, requires the observation model in Equation (13) in Annex A. The observation model for inter-beacon measurement, ri,j, between beacons iand j, is as follows: hIM i,j(~xk) = q(xj,k−xi,k)2+(yj,k−yi,k)2(1) where [xi,k,yi,k]Tis the estimate of the location of beacon iat time k. If beacon iis still in the beacon initialization stage, zr,iis used to initialize or update the initialization of beacon i. If it is in the core SLAM filter stage, zr,iis used in the update stage of the core SLAM filter using the following observation Jacobian: Hi,k=hxi,k−xk hr,i,yi,k−yk hr,i,0...0,xk−xi,k hr,i,yk−yi,k hr,i,0...i(2) All the terms in Hi,kare zero, except those for the entries corresponding to the robot and beacon i. Sensors 2014,14 7691 When integrating zi,j, different cases can be distinguished, depending on the stages of beacons iand j; see the summary in Figure 3. If both beacons are in the core SLAM filter stage, zi,jis used in the SLAM update stage using the following observation Jacobian: HIM i,j,k=h...0,xi,k−xj,k hi,j,yi,k−yj,k hi,j,0...0,xj,k−xi,k hi,j,yj,k−yi,k hi,j,0...i(3) All the terms in HIM i,j,kare zero, except those for the entries corresponding to beacons iand j. Figure 3. Diagram of the integration of inter-beacon measurements in the proposed scheme. is used in the initialization stage of beacon i is used in the initialization stage of beacon j NO Are both beacons in the core SLAM filter stage? Is beacon i in the core SLAM filter stage? Is beacon j in the core SLAM filter stage? NO YES YES YES NO is used to update the core SLAM filter If only one of the beacons, e.g., j, is in the core SLAM filter stage, zi,jis used in the initialization of the other beacon, e.g., i. If zi,jis the first measurement for beacon i, it is used to start the initialization of beacon i. For instance, in the case of using particle filters, zi,jis used to initialize PFiby deploying particles on a ring centered at [xj,k,yj,k]T. If neither beacon inor jare in the core SLAM filter stage, zi,jis kept for future use until the initialization of one of the two beacons finishes. Instead of buffering all measurements, the robot only keeps the mean value of the inter-beacon measurement—zav i,j—and the number of measurements—nmi,j. Assuming Gaussian noise of variance R, statistically speaking, zav i,jis another measurement of variance R/nmi,j. When the initialization of beacon ifinishes, it starts its core SLAM filter stage, and zav i,jis used for the initialization of beacon j. Therefore, when a beacon is initialized, low-variance inter-beacon measurements are integrated in the remaining beacon initialization tools, helping their convergence and originating a beacon initialization chain-reaction effect. As a result, using inter-beacon measurements drastically reduces the beacon initialization times, as will be shown in Section 6. The effect is more evident with higher values of IDL, since they originate a larger number of inter-beacon relations. 5. SLAM Supervision Module The SLAM supervisor receives as input metrics of the SLAM performance and dynamically selects the most suitable measurement gathering mode for the current conditions. Measurement gathering with high inter-beacon depth levels allows the reducing of map initialization times and the improving of map Sensors 2014,14 7692 estimation accuracy. However, once the map has been created, it is not interesting to reduce robot map error if we consider the increment in resources consumption they involve. On the other hand, changing measurement rates allows balancing between robot localization error and measurement and/or odometry noise levels. The following three measurement gathering modes can be clearly identified. Mapping mode: The objective is to accurately initialize the map as soon as possible. When SLAM starts, the map is assumed unknown: rapidly creating an accurate estimation of the map is very important. In fact, SLAM methods for rapid map initialization have attracted significant interest in recent years, as pointed out in Section 2. Creating a map requires initializing a good percentage of the available beacons. Measurement gathering with high IDL drastically reduces beacon initialization times and improves beacon localization accuracy. In the Mapping mode, the measurement gathering is configured with high IDL and with the highest possible measurement rate. The Mapping mode should be selected while the ratio between the number of non-initialized beacons and the number of initialized beacons is above a certain threshold. Relaxed mode: In this mode, an accurate map has already been created, and the robot can self-locate with low error. Thus, measurement gathering is configured with low rate and with IDL = 0 in order to save resources. The Relaxed mode is suitable with low odometry noise levels; the measurement rate can be reduced without impacting much on the robot location error. It is also suitable when the robot is at densely beacon-populated areas, where measurement gathering events collect a good number of measurements that allow accurate robot localization. On the other hand, the Relaxed mode is not suitable with high odometry noise levels, since high measurement rates should be used to compensate for the errors. The SLAM supervision module should select the Relaxed mode while the robot error is below a certain threshold. Of course, the ground-truth is assumed not available, and mode selection should be based on robot error estimation, as described below. Localization mode: Its objective is to allow accurate robot self-localization in cases with higher measurement and/or odometry noise levels. The map has been already created, and measurement gathering is configured with IDL = 0 and a high rate, in order to improve robot localization. This mode is suitable in cases where good map and robot accuracies are desired, of course at the expense of having a higher consumption of resources than the Relaxed mode. The SLAM supervision module should select the Localization mode, while the robot error is above a certain threshold. When the robot error falls below the threshold, the mode is changed to Relaxed in order to save resources. In this paper, the SLAM supervision module (see Figure 4) is implemented by a simple finite state machine with three states, one per each measurement gathering mode. The robot starts at the Mapping state and remains at this state until Cond1 is satisfied, i.e., until the ratio between the number of non-initialized beacons and the number of initialized beacons is below threshold T1. When the map initialization finishes, the state is changed to Localization. Of course, at any time, the robot can move to an unexplored area where new beacons are discovered. If Cond2 is satisfied, i.e., the ratio between the number of non-initialized beacons and the number of initialized beacons is above threshold T2, the state machine changes to Mapping, regardless of its previous state. Cond1 and Cond2 are expressed as: Sensors 2014,14 7699 performance evaluation between Rate2 and Rate1 with the different measurement noise levels analyzed in Section 6.1. Table 2. SLAM performance evaluation between Rate2 and Rate1 with different odometry levels (top) and with different measurement noise levels (bottom). Rate2 vs.Rate1 O1O2O3 Improvement in mean map error 31.58% 38.61% 48.64% Improvement in mean robot error 24.80% 35.43% 47.89% Improvement in mean beacon initialization time 6.99% 12.53% 8.01% Increase in mean computational burden 113.66% 121.96% 120% Rate2 vs.Rate1 σm,1σm,2σm,3σm,4σm,5 Improvement in mean map error 30.16% 30.50% 32.97% 31.40% 33.82% Improvement in mean robot error 24.45% 21.96% 25.56% 24.58% 28.65% Improvement in mean beacon initialization time 3.07% 6.89% 6.49% 11.72% 21.97% Increase in mean computational burden 135.98% 141.5% 134.35% 135.12% 139.20% As shown in Table 2(top), the measurement rate has a significant impact on mean robot error improvements. Robot error improvements are moderate with medium odometry noise levels, but increase up to 47.89% with bad odometry O3. With good odometry, Rate1 succeeds in obtaining good map and robot errors with low resource consumption. However, with bad odometry, the high distance between consecutive measurements in Rate1 is insufficient to keep the robot error low. This tendency can also be noticed in the map error. However, the performance improvement between Rate2 and Rate1 as odometry worsens is more evident in the robot error than in the map error. As shown in Table 2(bottom), SLAM performance improvements are very steady, regardless of the measurement noise level. As expected, the odometry noise level and the measurement noise level have a low influence on beacon initialization times and on computational burden. 6.3. Configuration of the Supervisor In SLAM, no previous knowledge is assumed: it is not possible to set in advance the measurement gathering settings. To cope with that, the SLAM supervisor analyzes the SLAM performance and dynamically modifies the measurement gathering configuration. The design of the SLAM supervisor was described in Section 5. This section selects the settings and parameters of the SLAM supervisor used in the experiments. The objective of the Mapping state is to accurately estimate the map as soon as possible. Depending on the hardware used, the constraint can be the robot computational capability, the sensor nodes capability or, more rarely, the bandwidth required by the measurement collection protocol. In the experiments, Mapping was configured with IDL = 2 and Rate2. The state machine remains at Mapping until the last beacon has completed its initialization. Then, the state machine changes to Localization. Furthermore, if a new beacon is discovered, it changes to Mapping, regardless of its previous state. Sensors 2014,14 7700 In the Localization state, the measurement gathering module is configured with IDL = 0 and Rate2, while in the Relaxed state, it is configured with IDL = 0 and Rate1.Cond3 and Cond4 in Section 5 establish the transitions between Localization and Relaxed. After analyzing the preliminary experiments, the following values were selected: T3=0.8 and T4=T3+0.3. The hysteresis between T3and T4was selected as H=0.3. To prevent noise in robot error estimation, in the experiments, the trace of the robot location covariance matrix—Σk,robot —was smoothed with a simple first order filter. 7. Experimentation and Validation This section experimentally evaluates the proposed scheme and compares it with the following SLAM methods. Method1 is the typical traditional SLAM scheme. During the whole experiment, it integrates only direct robot-beacon measurements (IDL = 0) taken at Rate2.Method2 is a non-adaptable method that exploits the advantages of integrating inter-beacon measurements. It configures measurement gathering with IDL = 2 and Rate2 during the whole experiment. Method3 is an adaptable scheme that configures measurement gathering with IDL = 2 and Rate2 during map initialization and, with IDL = 0 and Rate1, once the map has been initialized. Table 3summarizes their configuration. Table 3. Description of the methods’ configuration. During Mapping After Mapping Method1 IDL = 0, Rate2 IDL = 0, Rate2 Method2 IDL = 2, Rate2 IDL = 2, Rate2 Method3 IDL = 2, Rate2 IDL = 0, Rate1 Proposed scheme (PS) IDL = 2, Rate2 IDL = 0, Rate1/Rate2 It should be noticed that all these methods could be very easily implemented by the proposed scheme simply by changing the state transitions parameters in the SLAM supervisor. For instance, Method1 could be implemented by enforcing that the finite machine is always at state Localization. Furthermore, Method3 is a particular case of the proposed scheme with only states Mapping and Relaxed. From this point of view, our method generalizes existing techniques, adding flexibility and reactivity. The validation experiments were performed in the CONET Integrated Testbed. A validation data set similar to that in Section 6was created with 80 different robot paths and scenario configurations. The Nanotron NanoPAN range sensors were configured with a maximum sensing range of 8 m. All the methods were executed in exactly the same conditions: all are based on the PF-EKF scheme described in Annex Aand use the same EKF and PF parameters. 7.1. Evaluation and Comparison Figure 10 shows the result of applying the proposed SLAM scheme in one experiment. Ground-truth robot and beacon locations are in blue, while the resulting estimations are in red. The robot initial and final locations are marked in the figure with a black circle and a black rectangle, respectively. Figure 11 details its operation during the execution of the experiment. It shows: (top) the values of the Sensors 2014,14 7701 smoothed robot trace used as robot error estimation; (center) the measurement gathering state provided by the SLAM supervisor; and (bottom) the robot and map location errors at each time, represented in blue and red, respectively. Figure 10. Results of the proposed SLAM scheme in one experiment. Estimations are in red and the ground-truth in blue. −10 −5 0 5 10 15 −10 −5 0 5 10 x [m] y [m] Figure 11. Details of the operation of the proposed scheme along the experiment: (top) values of the smoothed robot trace; (center) the measurement gathering state; (bottom) robot and map location errors. 0 Relaxed Localization Mapping 1 1,5 2 0 100 200 300 400 500 600 700 800 900 1000 0 50 100 150 Time [s] Smoothed robot trace Mapping mode Localization mode Relaxed mode Robot error [cm] Map error [cm] It starts at t=0 s at state Mapping and remains at that state; see Figure 11, until the particle filters of all the discovered beacons converge, which occurred at t=55.6 s. Figure 10 shows in green the path Sensors 2014,14 7702 followed by the robot while the scheme is at the Mapping state. Due to the sensors’ maximum sensing range, the robot was capable of integrating direct robot-beacon measurements only from 55% of the 20 beacons deployed in the scenario. The particles filter of the beacons distant from the robot’s sensing range (45% of the beacons) converged using only inter-beacon measurements. As expected, the Mapping state, with IDL = 2 and Rate2, rapidly created an accurate map. At t=55.6 s, the last PF converged; the map was initialized with a map error of 65.5 cm, and the state machine changed to Localization. From that time on, the map error is represented in Figure 11. After the Mapping state, the smoothed robot trace was very low (meaning accurate estimation), fulfilling Cond3, and thus, when the last PF converged, the state machine changed to Localization and immediately changed again to Relaxed. The state machine remained at Relaxed until t=70.4 s, in which the smoothed robot trace grew above T4, satisfying Cond4: the state machine changed to Localization. The robot trace grew, because the beacons that surrounded the robot at those times had significantly high errors, higher than the mean map error. Integrating their measurements increased the robot error. After changing to the Localization state, the increase in the number of measurements integrated in SLAM (originated by changing from measurement rate from Rate1 (Relaxed) to Rate2 (Localization)) rapidly decreased the smoothed robot trace. At t=101 s, it fell below T3, fulfilling Cond3, and the state changed again to Relaxed. The Relaxed state remained until t=162 s. The robot motion had two consecutive curves, which suddenly degraded the robot odometry. This degradation in odometry originated a sudden increase in the robot trace. At t=162 s, the smoothed robot trace became above T4, fulfilling Cond4, and the state machine changed to Localization. With a higher measurement gathering rate, the robot location uncertainty improved; and at t=219.8 s, the smoothed robot trace fell below T3, and the state changed again to Relaxed. In the rest of the experiment, the smoothed robot trace increased and reduced as the robot followed its path. The state was changed when necessary reacting and adapting to the changing conditions. At the end of the experiment, the robot and map errors w.r.t. the ground-truth were, respectively, 32.4 cm and 14.3 cm, enough for a wide variety of applications. In the full experiment, the state machine was at the Mapping state during 7.60% of the total time, at Localization during a total of 17.24% and at Relaxed during a total of 75.16%. It was at the Relaxed state during the greater majority of the time involving the low consumption of resources. The same experiment was repeated, obtaining similar results. A video illustrating another experiment can be seen at [30]. In the following, the proposed scheme (PS in the figure) is compared with Method1,Method2 and Method3. All the methods were executed with each experiment in the validation data set with exactly the same parameters. Figure 12 shows the mean beacon location errors (top) and mean robot location errors (bottom) in 50 experiments. Figure 13 shows the mean beacon initialization times (top) and the computation times required (bottom) in each of the 50 experiments. Sensors 2014,14 7703 Figure 12. Mean beacon location errors (top) and mean robot location errors (bottom) in 50 experiments performed with Method1,Method2,Method3 and the proposed scheme (PS). 0 10 20 30 40 50 20 40 60 Experiment # Map error [cm] PS Method1 Method2 Method3 0 10 20 30 40 50 30 40 50 60 70 Experiment # Robot error [cm] PS Method1 Method2 Method3 Figure 13. Mean beacon initialization times (top) and computational times (bottom) required in each of the 50 experiments performed with Method1,Method2,Method3 and the proposed scheme (PS). 0 10 20 30 40 50 0 50 100 150 Experiment # Mean initialization time [s] PS Method1 Method2 Method3 0 10 20 30 40 50 0 10 20 30 40 Experiment # Computation time [s] PS Method1 Method2 Method3 Method2, which uses IDL = 2 and Rate2 during the whole experiment, obtained the lowest map and robot mean errors. Method1, which uses IDL = 0 and Rate2 during the whole experiment, obtained the worst map and robot errors. Method3 and the proposed method obtained intermediate map and robot Sensors 2014,14 7704 errors. The errors in the proposed method were lower than in Method3. Besides the Mapping state, which is the same for both, the proposed method has two states, Localization and Relaxed, that allow flexibility to balance between accuracy and burden, while Method3 only has one state. The Localization state enables the proposed method to reduce errors when necessary, involving very low extra costs. Method2,Method3 and the proposed method had similar mean beacon initialization times, since they all operate with IDL = 2 and Rate2 during map initialization. In contrast, Method1 uses IDL = 0 obtained beacon initialization three times higher. The robot computational burden of Method2 was three times higher than that of Method1,Method3 or the proposed method. It should be noticed that both, Method3 and the proposed scheme, require lower robot burden than Method1, the traditional method. Although both consume more resources during map initialization, they rapidly obtain an accurate map estimation that allows them to be at the Relaxed state, saving resources, during large periods of the experiment. As a result, in total, Method3 and the proposed scheme obtain lower robot and map errors with lower burden than Method1. Furthermore, the computational burden of the proposed method is a bit higher (6.5%) than that of Method3, since the proposed scheme goes in the Localization state from time to time in order to the reduce robot error, while Method3 keeps at the Relaxed state during all the time after map initialization. Method2 obtained the lowest map and robot mean errors, but required too high of a robot computational burden. The best tradeoffs between performance and burden were Method3 and the proposed method (PS). Both overtook Method1. In particular, the proposed method had lower map (34%) and robot error (14%) than Method1, requiring also lower computational burden (16%). When comparing with Method3, the proposed method had lower map and robot error than Method3 (19% and 31%, respectively) requiring only a bit higher computational burden (6.5%). The proposed scheme is also efficient in terms of the energy consumed by the beacons. We obtained a consumption model using the information provided by the manufacturer. The radio module is responsible for the greater part of the consumption. Each measurement takes 12 ms, during which, the emitter and the receiver interchange the measurement request and response packets. Both beacons transmit during 6 ms (consuming 210 mA) and receive during 6 ms (consuming 51 mA). The consumption of both beacons is the same and depends on the number of measurements in which they are involved. Table 4shows the mean number of measurements in which beacons are involved in the 50 experiments analyzed. It also shows the mean beacon energy consumptions in these experiments. As expected, Method2 consumed 360% more than the rest. The consumption of the proposed scheme was a bit higher than Method1 (3.9%) and than Method3 (1.7%). However, these small differences are compensated by the significant improvements in accuracy. Table 4. Comparison of beacon energy consumption of Method1,Method2,Method3 and the proposed scheme during the experiments. Proposed Scheme Method1 Method2 Method3 # of measurements involving a beacon 1,830 1,760 8,300 1,800 Mean beacon energy consumption (J) 11.53 11.09 52.29 11.34 Sensors 2014,14 7705 7.2. Robustness This section experimentally evaluates the robustness of the proposed scheme against measurement and odometry noise levels using the experiments in the validation data set. The measurements and odometry with different noise levels were obtained by adding noise to the real measurements and odometry as in Section 6. Figure 14 shows the robustness in the mean map error (full lines) and robot error (dashed lines) against measurement noise with standard deviation in the range [0.1–1.3] m. Method2 exhibited the best robustness both in map and robot errors. It uses IDL = 2 and Rate2 and integrates a very high number of measurements, achieving good robustness to measurement errors. Method1 and Method3 obtained the worst robustness. Method1 integrates only direct measurements, lacking the relations established by inter-beacon measurements and being more sensitive to measurement errors. In Method3, once the map has been initialized, measurement gathering is configured with Rate1, which is rather sensitive to measurement errors. Figure 14. Robustness of Method1,Method2 and Method3 and the proposed scheme (PS) against measurement noise level: the mean robot errors are in full lines, while the mean map errors are in dashed lines. 0.1 0.4 0.7 1 1.3 0 20 40 60 80 Measurement noise standard deviation [m] Map and robot errors [cm] PS Method1 Method2 Method3 Method3 Method2 Method1 Method3 Method2 Method1 PS PS The proposed scheme has moderate robustness against measurement noise. It is more robust than Method1 and than Method3. It includes the Localization state, designed to obtain good accuracy in the presence of high measurement and/or odometry noise levels. This is why it is more robust than Method3 when estimating robot location. Besides, its advantages over Method3 in map error are more evident with higher measurement noise levels (standard deviations higher than 0.7 m). Figure 15 shows the robustness of the mean map error (full lines) and mean robot error (dashed lines) when using the odometry, O1,O2and O3, defined in Section 6. Odometry O1corresponds to “1” in the abscissa axis. O2and O3correspond to “2” and “3”. Method3 had the worst robustness against odometry noise. After map initialization, Method3 always uses Rate1, which is very sensitive to odometry noise. The best robustness was obtained with Method2. On the other hand, although the odometry noise level has a moderate influence on map error, Method1 is sensibly less robust than the rest: the rest of the Sensors 2014,14 7706 methods integrate inter-beacon measurements during map initialization, making map estimation more robust to odometry noise. Figure 15. Robustness of Method1,Method2 and Method3 and the proposed scheme (PS) against odometry noise level: the mean robot errors are in full lines, while the mean map errors are in dashed lines. 1 2 3 0 20 40 60 80 100 120 Odometry noise level Map and robot errors [cm] PS Method1 Method2 Method3 Method2 Method3 Method2 Method1 Method1 Method3 PS PS Method2 has the best robustness, but, as analyzed in Section 7.1, it involves a very high computational burden. Method1 and Method3 have poor robustness. The proposed scheme has better robustness than Method1 and than Method3. 8. Conclusions This paper presents a RO-SLAM scheme that actuates over the measurement gathering process. It includes a measurement gathering module that implements a protocol by means of which it can take and collect at configurable rates direct robot-beacon and inter-beacon range measurements of configurable depth levels. It also includes a supervision module that monitors the SLAM performance and dynamically selects the configuration of the measurement gathering module. The proposed scheme exploits the capability of beacons of taking range measurements and transmitting them using ad hoc protocols. Besides, it exploits the flexibility of dynamically modifying the measurement gathering rate and inter-beacon depth level, allowing its adaptation to changing conditions. To the best of our knowledge, it is the first active RO-SLAM method that actuates over the measurement gathering process integrating inter-beacon measurements. The SLAM supervision module implements a simple finite state machine with three states, Mapping, Localization and Relaxed, each of them with different inter-beacon depth levels and measurement rates. The SLAM performance metrics selected are the percentage of discovered, but not initialized, beacons and the estimation of the robot error using the trace of the robot location covariance matrix. The proposed scheme and its modules are general and can be applied to any SLAM filter. In this paper, it has been applied to a PF-EKF SLAM filter, which is maybe one of the most common RO-SLAM methods. It has been validated in simulations and experiments performed in the CONET Integrated Sensors 2014,14 7707 Testbed, and its performance has been compared to the traditional methods based on integrating only direct robot-beacon measurements. The experiments show how the SLAM supervision module dynamically changes the state, changing the configuration of measurement gathering, impacting on which measurements are integrated in the SLAM filter. Its evaluation evidences the advantages of its adaptation and configuration capabilities. In these experiments, it obtained map and robot location errors significantly lower (e.g., 34% and 16%, respectively) than traditional RO-SLAM techniques, requiring 16% lower computational burden and similar beacon energy consumption. Besides, it exhibited better robustness against measurement and odometry noise levels. The proposed scheme allows high flexibility, being able to implement a high number of methods by simply modifying the parameters of the SLAM supervisor. Thus, it can be easily adapted to particular problems or applications. This work opens wide fields for research. The SLAM supervisor used in this paper is implemented by a simple finite state machine with only three states. More complex SLAM supervisors could be devised in order to enable more refined adaptation to concrete conditions. Acknowledgments This work has been supported by the EC-SAFEMOBIL (European Commission ICT-2011-288082), CLEAR (DPI2011-28937-C02-01) and the Ministerio de Educaci´ on y Deportes FPU Program. Conflicts of Interest The authors declare no conflicts of interest. A. Appendix A: PF-EKF SLAM in a Nutshell This annex briefly summarizes the PF-EKF SLAM filter used in the experiments. The proposed scheme can be applied to any SLAM filter. This is why it has been preferred to present the well-known PF-EKF SLAM filter in a condensed way. For a more detailed description, refer to [29]. EKF-SLAM is a well-known technique that recursively solves the online SLAM problem where the map is feature-based. It estimates, at the same time, the robot pose and the position of the beacons. Thus, the state vector adopted is: ~xk= [rpk,x1,k,y1,k,...,xn,k,yn,k]T(10) where rpkis the robot pose and [xi,k,yi,k]Tis the 2D location of static beacon iat time k. The robot pose is usually taken as rpk= [xk,yk,θk]T, the robot 2D position and heading at time k. During the EKF-SLAM prediction phase, the mean µkand covariance Σkof the state vector ~xkare updated as follows: µ− k=f(µk−1,uk)(11) Σ− k=AkΣk−1AT k+Qk(12) where Qkis the covariance matrix that models the uncertainty in f(~xk−1,uk), the robot state transition model. fis often non-linear, and its Jacobian Ak=∂f ∂~xk−1is used. Sensors 2014,14 7708 When the robot takes at time ka new robot-beacon range measurement, zr,i, from beacon i, the update phase of the EKF can be performed using the observation model, hi, which expresses the distance between the robot and beacon i: zr,i=hi(~xk) = q(xk−xi,k)2+(yk−yi,k)2(13) Ri,kis the variance of the uncertainty in hi(~xk). Hence, the mean and covariance of the state can be updated as: µk=µ− k+Kk(zr,i−hi(µ− k)) (14) Σk= (I−KkHi,k)Σ− k(15) where Hi,kis the Jacobian of hiat time kand Kkis the Kalman gain. In PF-EKF SLAM, the partial observation problem is solved using particle filters (PFs) for beacon initialization. As analyzed in Section 2, PFs are one of the most widely used beacon initialization tools, due to their flexibility and robustness. PFs can represent any probability distribution and can naturally solve the partial observation problem, due to their multi-hypothesis capability. The integration of PFs and the EKF is as follows. When the robot receives the first range measurement, zr,i, from beacon i, it initializes an auxiliary PF for beacon i,PFi, in which the particles, which are hypotheses of the beacon location, are spread around the robot at distances drawn from an annular distribution centered at the robot current location with mean zr,iand a width that depends on the measurement variance. When the robot receives a new measurement from beacon i, the particles of PFiare updated, making them condensate towards the beacon position. PFs implement the so-called importance resampling, which draws Mparticles with replacement. The probability of drawing each particle is given by its importance weight. Importance resampling helps to have a faster convergence of the PFs. We considered that a PF has converged in a Gaussian distribution when the covariance of its particles is lower than a certain value. In the experiments shown in this paper, PFs were assumed with 300 particles, and importance resampling replaced M=10% of the particles. When PFiconverges, the beacon location, [xi,yi]T, is computed as the weighted mean of all particles, and it is added to the EKF state vector, ~xk. Thus, along the PF-EKF SLAM iterations, each beacon can be in two stages: the PF stage and the EKF stage. When the robot receives a measurement from a beacon, it will be used to update its PF or the EKF, depending on the beacon stage. The flexibility of PFs is at the expense of computational burden. PF iterations require significantly more burden than EKF iterations. That is why PF convergence is often taken as a metric for computational cost in PF-based SLAM methods. References 1. Challa, S.; Leipold, F.; Deshpande, S.; Liu, M. Simultaneous Localization and Mapping in Wireless Sensor Networks. In Proceedings of the Intelligent Sensors, Sensor Networks and Information Processing Conference, Melbourne, Australia, 5–8 December 2005; pp. 81–87. 2. Menegatti, E.; Zanella, A.; Zilli, S.; Zorzi, F.; Pagello, E. Range-Only SLAM with a Mobile Robot and a Wireless Sensor Networks. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA’09), Kobe, Japan, 12–17 May 2009; pp. 8–14.