Robot-Beacon Distributed Range-Only SLAM for Resource-Constrained Operation

sensors Article Robot-Beacon Distributed Range-Only SLAM for Resource-Constrained Operation Arturo Torres-González, Jose Ramiro Martínez-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; arturotorr[email protected] (A.T.-G.); [email protected] (A.O.) *Correspondence: [email protected]; Tel.: +34-954-487-357; Fax: +34-954-487-340 Academic Editor: Gonzalo Pajares Martinsanz Received: 8 February 2017; Accepted: 11 April 2017; Published: 20 April 2017 Abstract: This work deals with robot-sensor network cooperation where sensor nodes (beacons) are used as landmarks for Range-Only (RO) Simultaneous Localization and Mapping (SLAM). Most existing RO-SLAM techniques consider beacons as passive devices disregarding the sensing, computational and communication capabilities with which they are actually endowed. SLAM is a resource-demanding task. Besides the technological constraints of the robot and beacons, many applications impose further resource consumption limitations. This paper presents a scalable distributed RO-SLAM scheme for resource-constrained operation. It is capable of exploiting robot-beacon cooperation in order to improve SLAM accuracy while meeting a given resource consumption bound expressed as the maximum number of measurements that are integrated in SLAM per iteration. The proposed scheme combines a Sparse Extended Information Filter (SEIF) SLAM method, in which each beacon gathers and integrates robot-beacon and inter-beacon measurements, and a distributed information-driven measurement allocation tool that dynamically selects the measurements that are integrated in SLAM, balancing uncertainty improvement and resource consumption. The scheme adopts a robot-beacon distributed approach in which each beacon participates in the selection, gathering and integration in SLAM of robot-beacon and inter-beacon measurements, resulting in significant estimation accuracies, resource-consumption efficiency and scalability. It has been integrated in an octorotor Unmanned Aerial System (UAS) and evaluated in 3D SLAM outdoor experiments. The experimental results obtained show its performance and robustness and evidence its advantages over existing methods. Keywords: robot-sensor network cooperation; SLAM; sensor networks 1. Introduction This paper deals with Range-Only (RO) Simultaneous Localization and Mapping (SLAM) in which the robot uses range measurements to build a map of an unknown environment and to self-localize in that map. RO-SLAM does not require line-of-sight operation and is independent of lighting conditions, whereas visual SLAM is sensitive to them and is not suitable in the presence of dense dust or smoke [ 1 ]. Using radio beacons as landmarks naturally solves the data association problem, typical of visual SLAM. Besides, RO-SLAM operates with shorter measurement and state vectors than visual SLAM, resulting in significantly lower computational burden. Our envisioned application is aerial robot autonomous navigation for maintenance and repairing in industrial plants. We are interested in RO-SLAM schemes where the robot uses nodes from a sensor network as landmarks. Consider a GPS-denied scenario where a large number of sensor nodes (beacons) have been deployed at unknown static locations. For instance, they have been placed at random locations for real-time monitoring an accident, or they are used for monitoring an industrial facility, and their exact location was not registered during deployment. This is not a Sensors 2017,17, 903; doi:10.3390/s17040903 www.mdpi.com/journal/sensors Sensors 2017,17, 903 2 of 19 constraint: sensor nodes are already used for monitoring and maintenance in many industrial plants. Each node periodically gathers and filters measurements and transmits them to a monitoring station. We assume that each node can measure the distance to other nodes. This is not a limitation, in fact most Commercial Off-The-Shelf (COTS) nodes can measure the Radio Signal Strength of Incoming packets (RSSI) and estimate the range to the emitting node [ 2 ]. Mapping and robot localization is essential for UAS navigation in the above GPS-denied environments. Besides, knowing the location of beacons enables robot-sensor network cooperative missions of interest in these scenarios such as sensor node transportation and deployment [ 3 , 4 ], collection of data from sensors [ 5 , 6 ] or node remote powering [ 7 ]. The potential capabilities of robot-sensor network collaboration have recently originated significant interest in RO-SLAM methods that use sensor nodes as beacons [ 8 – 11 ]. However, most existing techniques consider beacons as passive landmarks disregarding the computational, communication and sensing capabilities with which they are actually endowed. The performance of RO-SLAM methods improves as the number of measurements integrated in SLAM increases. For instance, integrating beacon-beacon (inter-beacon) measurements improves map and robot estimation accuracy [ 12 ]. However, using more measurements requires spending more resources such as the energy, bandwidth and computational power required to gather, transmit and integrate the measurements. SLAM is a resource demanding task. In the above scenarios the robot and beacons perform many other tasks apart from SLAM, all of them sharing the available resources. Besides, the available resources can be, and usually are, constrained by technological or application limitations such as the available energy and computational capacity of beacons or the bandwidth of the sensing channel. Resource consumption reduction has attracted high interest in SLAM, but very few resource-constrained RO-SLAM methods have been reported. This paper presents a RO-SLAM scheme that is capable of exploiting robot-beacon cooperation in order to improve SLAM accuracy and efficiency while meeting a given resource consumption bound. In this work, the resource consumption bound is expressed in terms of the maximum number of measurements that can be integrated in SLAM per iteration. The sensing channel capacity used, the beacon energy consumed or the computational capacity employed, among others, are proportional to the number of measurements that are gathered and integrated in SLAM. The proposed scheme can meet static and dynamic bounds, e.g., determined by an online resource allocation tool, enabling high flexibility, which can be of interest in many cases. Our scheme employs a distributed Sparse Extended Information Filter (SEIF) SLAM method, in which each beacon gathers and integrates in the SLAM update stage robot-beacon and inter-beacon measurements. Like SEIF algorithm, our scheme can be executed in constant-time. Our scheme also comprises a distributed tool that uses a gain-cost analysis to dynamically select the most informative measurements to be integrated in SLAM. The proposed scheme has a robot-beacon distributed approach were beacons actively participate in measurement selection, gathering and integration in SLAM. It has the following main properties: (1) adaptive resource-constrained operation, since it dynamically adapts to satisfy the given resource consumption bound; (2) accuracy, since it integrates inter-beacon measurements, significantly improving map and robot localization accuracies and speeding up beacon initialization; (3) efficiency, since it gathers and integrates the most informative measurements and; (4) scalability, since all the involved tasks are executed in a distributed manner. This paper presents the scheme, evaluates and compares its performance with existing methods in experiments performed with an octorotor UAS. The proposed scheme employs the distributed SEIF SLAM that we sketched in [ 13 ]. In this paper, it is enhanced and combined with a distributed measurement selection tool that enables resource-constrained operation. The main improvements in this paper over [13] are: • development of a distributed robot-beacon tool that selects the most informative measurements that are integrated in SLAM fulfilling the resource consumption bound; • harmonious integration of the distributed SEIF SLAM and the measurement selection tool. The resulting scheme outperforms that in [13], as described in Section 6; •extension to 3D SLAM, integration and experimentation of the scheme with an octorotor UAS; Sensors 2017,17, 903 3 of 19 •new experimental performance evaluation and comparison with existing methods; •new subsection with experimental robustness evaluation; • extension and more detailed related work. Furthermore, the paper has been restructured and all sections have been completed and rewritten for clarity. The paper is organized as follows. Section 2summarizes the related work. Section 3presents the problem formulation. Sections 4and 5briefly describe the operation of the robot and beacons. Section 6 presents its evaluation, comparison and robustness analysis in 3D SLAM experiments. The conclusions are in Section 7. 2. Related Work As it is known, in Simultaneous Localization and Mapping (SLAM) a robot builds a map of an unknown environment while simultaneously keeping track of its location within that map. RO-SLAM methods solve the SLAM problem using only range measurements. Recursive Bayesian Filters (RBFs) provide a solid statistical basis for estimation integrating measurements in presence of noise. A good number of RO-SLAM methods have been developed in the last years. Most of them employ approaches based on the Extended Kalman Filter (EKF) SLAM or on Fast-SLAM. These filters have been combined with different initialization tools in order to deal with the partial observability of range measurements. Particle Filters (PFs) [ 14 ] involve significant computational burden but provide accurate results. Probability grids [ 12 , 15 ] have also been used but are computationally inefficient to implement in 3D. Trilateration [ 9 ] is the simplest approach but it can lead to high initialization errors, which can originate inconsistent estimations. Gaussian mixtures [ 16 , 17 ] provide a multi-hypothesis representation that allows integrating measurements from the beginning in undelayed SLAM methods. Dual to Kalman Filters, Information Filters represent the state by its information vector ξ and its information matrix Ω . The Sparse Extended Information Filter (SEIF) [ 18 ] for online SLAM maintains a sparse representation of Ω . SEIF is naturally efficient in the update stage and the sparseness of Ω enables using efficient algorithms for motion update and state recovery. SEIF can be executed in constant time regardless of the map size. Most RO-SLAM techniques employ only robot-beacon range measurements. Work [ 12 ] was the first in using also measurements between beacons (inter-beacon). Integrating inter-beacon measurements improves map and robot estimation accuracy and speeds up beacon initialization. Despite its advantages few methods employ them. Integrating more measurements involves higher consumption of energy, bandwidth and processing time to gather, transmit and integrate them in SLAM. Work [ 19 ] integrates inter-beacon measurements adopting decentralized methods to deal with the increase in the consumption of resources. Efficiency in the use of resources has received high interest in SLAM. Some methods use decision making tools based on information metrics to balance efficiency and accuracy. Work [ 20 ] developed an approach that selects highly informative loop-closure links to simplify the Pose SLAM information form and reduce its computational cost. They also achieve a compact information matrix reducing the number of measurements by operating in open loop for long periods. This open loop operation requires good odometry estimations, which are not always available in all scenarios and applications. Work [ 21 ] presented a method for estimating the quality of visual measurements in single-camera SLAM maximizing the mutual information between measurements and states in order to select the best camera measurements. Despite its potential advantages, very few SLAM techniques, and even less RO-SLAM, for resource-constrained operation have been reported. In work [ 22 ] we proposed a RO-SLAM scheme that improves efficiency by choosing between two measurement gathering modes. Mode selection is performed using a simple centralized tool based on heuristics of robot and map estimation uncertainty: It does not consider resource consumption requirements, hence it cannot adapt to given resource consumption bounds. Besides, mode decision is fully centralized at the robot, lacking scalability. The scheme proposed in this paper is capable of satisfying a given resource consumption bound. It uses a distributed measurement selection tool that Sensors 2017,17, 903 4 of 19 prioritizes measurements analyzing cost and expected uncertainty improvement. Besides, the scheme is distributed and beacons actively participate in measurement selection, gathering and integration in SLAM. 2.1. Range Only SEIF SLAM in a Nutshell This section briefly summarizes the SEIF SLAM algorithm as an introduction to the proposed scheme. Most expressions have been omitted. For notation simplicity, time subindex t has also been omitted. Refer to [23] for further details. RO-SLAM methods commonly adopt a state vector ~ x comprised of the robot position ( xr ) and the location of all the beacons in the map ( x1 , x2 , . . . , xN ). SEIF is based on Extended Information Filter (EIF). Duals to Kalman Filters, Information Filters (IFs) employ the canonical representation of multivariate Gaussian distributions. The standard representation is based on the mean vector µ and the covariance matrix Σ , whereas the canonical representation is based on the information vector ξ=Σ−1µand the information matrix Ω=Σ−1. Ω=      Ωxr,xrΩxr,x1· · · Ωxr,xN Ωx1,xrΩx1,x1· · · Ωx1,xN . . .. . ..... . . ΩxN,xrΩxN,x1· · · ΩxN,xN       (1) The information matrix Ω is symmetric and positive-semidefinite. Each off-diagonal entry of Ω , called the link [ 18 ], represents the relation between two elements in ~ x . At any time in the SEIF SLAM operation some of the off-diagonal elements of Ω are zero meaning lack of information between the involved elements; some of them have high values, called strong links, meaning high amount of information; and a number of them have values close to zero, called weak links. Weak links have lower impact on the estimation of ~ x than strong links but both involve similar computational burden. SEIF operates as EIF but maintains a sparse representation of Ω by keeping the number of active beacons bounded by a threshold. At each step the discovered beacons are classified in active and passive. Active beacons are those with non-zero links to the robot. Every time the number of active beacons is above the bound, the sparsification step is performed deactivating the beacons with the weakest links. EIF measurement update modifies only the entries of Ω corresponding to the elements involved in the measurement. Factorizing Ω allows efficient update stages regardless of the map size. SEIF inherits this efficiency. Furthermore, by bounding the number of active beacons, SEIF significantly reduces the computational burden in the prediction stage. For linearizing the prediction and observation models it is necessary to recover the state estimate µ from the predicted ¯ Ω and ¯ ξ . SEIF recovers only the useful part of ~ x , robot and active beacons. Of course, enforcing sparseness in Ω involves an approximation error in the estimations obtained by SEIF. Work [ 18 ] suggests using sparsification bounds in the range [4,5,7–11] to balance between accuracy degradation and burden reductions. 2.2. Integration of Range Measurements Range measurements have the problem of partial observability. To cope this it SEIF should be combined with an auxiliary beacon initialization mechanism. Our scheme uses PFs for beacon initialization due to their high flexibility. Each beacon can be in the “initialization phase” or in the “state vector phase”. In the “initialization phase” each beacon executes its own PF. When the PF converges, the beacon initializes its state vector: The “state vector phase” starts. The observation model used for range measurement zr,itaken by the robot to beacon biis: hr,i(µ) = qδ2 x+δ2 y+δ2 z, (2) where δx=µx−µi x , δy=µy−µi y and δz=µz−µi z . (µx , µy , µz) and ( µi x , µi y , µi z ) are respectively the estimated positions of the robot and of beacon bi.hr,iis nonlinear. Its Jacobian is: Sensors 2017,17, 903 5 of 19 Hr,i=∂hr,i ∂µ =hδx hr,i δy hr,i δz hr,i· · · −δx hr,i −δy hr,i −δz hr,i· · ·i(3) All the entries of Hr,i that are not shown in (3) are zero. Only the entries of Hr,i corresponding to the robot and beacon bi are not zero. The proposed method also integrates inter-beacon measurements, such as zi,j gathered by beacon bi to bj . The adopted model hi,j is an extension of that in (2) taking δx=µi x−µj x , δy=µi y−µj y and δz=µi z−µj z . The entries of its Jacobian Hi,j are zero except for those corresponding to the beacons involved in the measurement: Hi,j=h0· · · δx hi,j δy hi,j δz hi,j· · · −δx hi,j −δy hi,j −δz hi,j· · ·i(4) 3. Problem Formulation Consider a GPS-denied scenario where a number of sensor nodes endowed with sensing, computational and communication capabilities have been deployed at static locations. We assume that the location of the nodes is not known. For instance, they have been deployed at unknown locations or their exact location has not been registered during deployment. Each node is assumed equipped with a range sensor and can measure the distance to the robot or to other nodes within its sensing region. Range measurements are assumed affected by statistically independent Gaussian noise. We are interested in RO-SLAM techniques that use sensor nodes as radio beacons (landmarks), to online estimate the locations of sensor nodes and of the robot. The SLAM method can exploit the capabilities of the beacons deployed but, at the same time, should make efficient use of the available resources and should comply with resource consumption constraints. In conventional RO-SLAM techniques the robot gathers range measurements to the beacons within its sensing region and integrates these measurements in the update stage, see Figure 1a. In the adopted scheme measurement gathering and integration in SLAM is distributed among the beacons. The robot computes the SEIF SLAM prediction stage and transmits the predicted state to the beacons within its sensing region. Each beacon receiving the message: (1) gathers range measurements to the robot and other beacons (inter-beacon measurements), see Figure 1b; (2) integrates these measurements and computes its contribution to the update stage and; (3) transmits its contribution to the robot. Finally, the update stage of SLAM expressed in the information form is additive [ 23 ] and the robot reconstructs the updated state by simply adding the contributions it received. The proposed scheme naturally exploits robot-beacon cooperation to solve online SLAM: Accurately, since it integrates inter-beacon measurements; efficiently, distributing measurement gathering and integration in SLAM, and hence sharing burden and energy consumption; and in a scalable manner. Efficiency in the use of resources is very important in robot-sensor network cooperation. In most cases the SLAM algorithm is executed simultaneously with other tasks, all of them sharing the available resources. Furthermore, radio beacons gathering range measurements such as RSSI, Time of Arrival (ToA) or Differential ToA, make use of some kind of communication, which requires using a channel with a certain (constrained) capacity. In fact the capacity of the sensing channel is one of the most relevant constraints in settings with a high number and density of deployed beacons. In our problem resource consumption can be expressed in terms of the maximum number of measurements that are gathered and integrated in SLAM at each iteration. The use of the sensing channel, the consumption of beacons energy and of beacons computational capacity are proportional to the number of measurements that are gathered and integrated in SLAM. Hence, bounding the number of measurements constrains the consumption of the main resources involved. Sensors 2017,17, 903 6 of 19 b1 b5 b2 b3 b4 zr,1 zr,2 zr,3 (a) b1 b5b2 b3 b4 z1,r z3,r z3,2 z3,4 z2,r z2,3 z2,5 (b) Figure 1. Measurement gathering in conventional SLAM methods ( a ) and in the proposed distributed SLAM scheme ( b ). Gray circles represent the sensing regions of the robot ( a ) and beacons b1 , b2 and b3(b). A diagram of the proposed scheme with its main modules is shown in Figure 2. The scheme combines (1) a distributed SEIF SLAM method in which beacons gather and integrate in SLAM robot-beacon and inter-beacon measurements; and (2) a distributed information-driven measurement selection tool that dynamically selects the measurements that are integrated in SLAM in order to improve performance while fulfilling the bound in the total number of measurements. Both components are executed in a distributed manner by the robot and by the beacons. Each beacon maintains a local version of the SLAM state whereas the global state is maintained only by the robot. The message interchange and the operation of the robot and beacons is summarized in Figure 3. Methods that select the measurements that best reduce the uncertainty in the SLAM global state are necessarily centralized and have to deal with the information matrix of the global state. These methods incur in high computational burden with large maps and scale badly with the map size. The proposed scheme approximates this centralized measurement selection by a robot-beacon distributed tool that preserves the constant time execution and scalability. In Section 6.2 it is experimentally shown that the adopted tool is almost as accurate as the centralized measurement selection, adifference in map and robot RMS errors lower than 3%. The distributed measurement selection tool is performed in two steps: Measurement distribution and measurement allocation. In measurement distribution the robot dynamically decides the number of measurements that are assigned to each beacon within the robot sensing region using expectations of uncertainty reductions. In measurement allocation, each beacon bi decides the actual measurements that it will gather and integrate in SLAM analyzing the cost and expected uncertainty improvement of integrating each measurement. The bound in the number of measurements that can be gathered in each iteration is NMmax . As example where NMmax has a high value is shown in Figure 1b: Each beacon within the robot sensing region gathers and integrates a measurement to each beacon within its sensing region. An example with a low NMmax in shown in Figure 4. b1 and b2 are assigned with only one measurement and gather z1,r and z2,r . b3 is assigned with two measurements and besides z3,r , it gathers z3,2 , the measurement that achieves the best expected uncertainty improvement-cost trade-off. Sensors 2017,17, 903 7 of 19 SEIF SLAM Prediction Sum of update contributions Sparsification Measurement distribution Measurement allocation Measurement gathering Computation of update contribution Update request Robot Beacon Update response Figure 2. Diagram of the proposed resource-constrained RO-SLAM scheme. UpdateReq UpdateResp SEIF SLAM prediction Sum of update contributions Sparsification Measurement allocation Measurement gathering Robot Beacon bi Computation of update contributions Measurement distribution Figure 3. Operation and message interchange in the proposed scheme. b1 b5b2 b3 b4 z1,r z3,r z3,2 z2,r Figure 4. Measurement gathering with a low value of NMmax ; b1 gathers z1,r ; b2 gathers z2,r ; b3 gathers z3,rand z3,2. 4. Operation of the Robot The operation of the robot can be decomposed in four main tasks: (1) computation of the SEIF SLAM prediction stage; (2) reconstruction of the updated state using the contributions received by the robot; (3) computation of the sparsification step; and (4) measurement distribution. For brevity, most SEIF equations have been omitted. Refer to [23] for further details. The robot operation is as follows, see Algorithm 1. First, the robot computes the SEIF SLAM prediction. Static beacons and nonlinear robot kinematic model are assumed. The robot Sensors 2017,17, 903 8 of 19 Jacobian is computed at each time, which requires recovering the state. Our scheme uses the efficient algorithm described in [ 18 ] for motion update and state recovery. This algorithm computes the predicted information vector ¯ ξt and information matrix ¯ Ωt and recovers the predicted µt . Algorithm 1: Summary of the operation of the robot. Require: ξt−1,Ωt−1,NMmax,LM 1: SEIF motion update and state recovery 2: Create and broadcast UpdateReq message 3: Receive UpdateResp messages 4: Extract ξi,Ωiand uiifrom UpdateResp messages 5: Compute ξtand Ωtas in (6)–(7) 6: SEIF Sparsification 7: Measurement distribution. Create LM 8: return ξt,Ωt,LM As the robot moves beacons go in and out of the robot sensing region. The robot maintains BSr , a list with the beacons that are currently within its sensing region. At each time t the robot broadcasts an UpdateReq message that includes µt and LM , a list created by the robot at t− 1 that contains the number of measurements that have been assigned to each beacon bi∈BSr . Transmitting the whole µt in the UpdateReq message is not suitable in cases with large maps. Only the elements in µt required for each beacon are transmitted. Let evi be the vector with the estimates required by beacon bi to compute its contribution to the update stage: evi= [µrµiµj]T, (5) where µr is the estimation of the robot current location, µi is the estimation of the location of beacon bi and µjrepresents the estimations of the location of every beacon bjwithin the sensing region of bi. When bi receives the UpdateReq message, it performs as described in Section 5and transmits to the robot an UpdateResp message with ξi,t and Ωi,t , its contribution to the SLAM update stage. The robot reconstructs the updated state, ξtand Ωt, using ¯ ξtand ¯ Ωtand the update contributions it received: ξt=¯ ξt+∑ i FT iξi,t, (6) Ωt=¯ Ωt+∑ i FT iΩi,tFi, (7) where Fi is the projection matrix that implements the operations necessary to allocate ξi,t and Ωi,t at the suitable entries in ξtand Ωt. Next, the robot checks if Ωt satisfies the SEIF sparsification bound. If not, the beacons with the weakest links are deactivated as described in [18]. The final step performed by the robot is to distribute the number of measurements NMmax between bi∈BSr . NMmax is considered an input to our scheme. It can be static or dynamic, computed by an online resource allocation tool, for instance analyzing the capacity of the channel using link quality estimators, see e.g., [ 24 ]. Measurement distribution is performed proportionally to IGi,t , the usefulness of the measurements from beacon bi to reduce the uncertainty of the SLAM state. bi has impact on the SLAM state only if its update contribution reaches the robot, i.e., if bi receives the UpdateReq message sent by the robot and if the robot receives the UpdateResp message with the update contribution from bi . These two events are statistically independent. Taking pr,i as the Packet Reception Rate (PRR) from the robot to biand assuming symmetric PRRs, IGi,tcan be estimated as: IGi,t=p2 r,iuii,t, (8) Sensors 2017,17, 903 9 of 19 where uii,t estimates the capability of the measurements gathered by bi to reduce the uncertainty in the SLAM state. Transmission errors in sensor networks are not infrequent. This criterion naturally assigns more measurements not only to the most informative beacons, but also to those with better communication with the robot. Each bi computes its own uii,t , described in Section 5, and transmits it to the robot in an UpdateResp message. The robot can measure pr,i to each bi∈BSr by simply analyzing message transmission success. Next, the robot allocates the NMmax measurements among beacons bi∈BSr proportionally to IGi,tand creates LM, the list with the number of measurements assigned to each beacon bi∈BSr. 5. Operation of Beacons The operation of beacons is summarized in Algorithm 2. Once beacon bi has received the UpdateReq message it performs as follows: (1) executes measurement allocation and selects the most informative measurements; (2) gathers and integrates in SLAM the selected measurements and; (3) transmits to the robot its update contribution in an UpdateResp message. Algorithm 2: Summary of the operation of beacon bi 1: Receive UpdateReq message. Extract eviand LMi 2: Measurement allocation. Compute Ji,jand uii 3: Gather the LMimeasurements with the highest Ji,j. Create MSi 4: if (biis at “state vector phase”) then 5: Compute ξi,tand Ωi,twith MSias in (15)–(16) 6: else 7: Use MSito update the PF of bi 8: end if 9: Create UpdateResp message and transmit it to the robot 5.1. Measurement Allocation UpdateReq messages include LM . Once beacon bi has received an UpdateReq message, it extracts LMi , the number of measurements it was assigned with. Let BSi be the set of beacons bj within the sensing region of bi . In measurement allocation each beacon bi selects which measurements zi,j,bj∈BSi it should gather and integrate in SLAM. We adopt a common approach in information-driven measurement selection and formulate the problem as the greedy optimization of a utility function that establishes a trade-off between information gain and resource consumption: Ji,j=ri,j−αci,j(9) ci,j is the cost of the resources consumed in gathering and integrating measurement zi,j . In sensor networks energy is maybe the most constrained resource. We take ci,j as the energy consumed by bi in gathering and integrating zi,j . ci,j could be different for each beacon, e.g., depending on the remaining energy in its batteries. For simplicity, ci,j was assumed the same for all measurements. The reward ri,j=uii,j is the expected SLAM uncertainty improvement resulting after integrating zi,j . α is a weighting factor that balances the cost the reward. Its effects will be evaluated in Section 6.3. This cost-reward approach has been used in the literature, like in [ 25 ], where they used it to make decisions on a robot exploration problem. In our work, αwas determined experimentally. The reward ri,j=uii,j is determined as follows. In our distributed scheme each beacon maintains its own local state. Ωi,t is the predicted information matrix for time t of the local state of bi . Ωi,t was computed by bi at t− 1. It is easy to notice that the updated information matrix of the local state of bi that would result after integrating measurement zi,jis: Ω‘i,t=Ωi,t+Ωi,j,t(10) Sensors 2017,17, 903 16 of 19 The proposed method preserves the scalability of the distributed SEIF presented in [ 13 ]. Measurement distribution involves only the beacons within the robot sensing region, whereas measurement allocation, performed by each beacon bi , involves only the beacons within the sensing region of bi . Thus, the computational complexity depend on the beacon density, not on the map size. Beacons are used as landmarks in RO-SLAM: Highly inhomogeneous local beacon densities are not suitable in RO-SLAM. It is often more interesting if beacons are deployed in densities with some homogeneity, leading to constant time execution regardless of the map size. NMmax is taken as an input to our scheme. The performance of the proposed scheme with different values of NMmax is summarized in Table 2. The measurements from all the experiments were logged and the proposed scheme was offline executed with NMmax . The integration of measurements is critical for PF convergence and low values of NMmax decelerate PF convergence. On the other hand, the estimation accuracy was only slightly affected, which is attributed to its capability to select informative measurements. With NMmax = 80 the average number of measurements actually integrated in SLAM was similar to that with NMmax = 60. The explanation is the role of α . In the experiments all measurements are assumed to have the same cost. Thus, α acts as a threshold since measurement zi,jis gathered only if Ji,j=ri,j−αci,j>0. Table 2. Average performance of the proposed scheme with different values of NMmax. N Mmax =40 N Mmax =60 N Mmax =80 Map RMS error (m) 0.35 0.346 0.34 Robot RMS error (m) 0.52 0.51 0.51 PF convergence times (s) 15.8 9.5 5.7 # of measurements/iteration 40 56.5 61.7 The performance of the proposed scheme with different values of α is summarized in Table 3. α allows setting our scheme to prevent integrating measurements that are not very informative. With α= 1.5, almost all measurements satisfy Ji,j> 0 and the number of measurements integrated in SLAM is almost NMmax . With α= 15, many measurements do not satisfy Ji,j> 0 soon in the experiments and in average only 49.3 measurements were integrated per iteration. These were the two extremes in the range of α , an intermediate value α= 7.5 was used. Despite the difference in the number of measurements, the value of αaffects accuracy very slightly as shown in Table 3. Table 3. Average performance of the proposed scheme with different values of α. α=1.5 α=7.5 α=15 Map RMS error (m) 0.34 0.34 0.37 Robot RMS error (m) 0.51 0.51 0.52 PF convergence times (s) 5.7 5.7 5.9 # of measurements/iteration 78.9 61.7 49.3 The proposed distributed scheme needs communication between the robot and beacons. In this sense the transmission errors in sensor networks cannot be ignored. Its performance assuming different PRR levels is summarized in Table 4. Our method explicitly considers PRR in the estimation of IGi and assigns more measurements to the beacons that have better link quality with the robot. As expected, it exhibits good robustness to PRR. Even with PRR = 40%, transmission error rate of 60%, its performance is very slightly perturbed. Sensors 2017,17, 903 17 of 19 Table 4. Average performance of the proposed scheme with different PRR levels. PRR = 40 PRR = 60 PRR = 80 PRR = 100 Map RMS error (m) 0.4 0.37 0.35 0.35 Robot RMS error (m) 0.57 0.53 0.52 0.51 PF convergence times (s) 9.6 7.1 6.4 5.7 # of measurements/iteration 44.8 51.4 57.1 61.7 7. Conclusions RO-SLAM has some characteristics that make it more suitable than visual SLAM in a wide range of robot-sensor node cooperative missions. Most RO-SLAM methods consider beacons as passive landmarks and do not exploit the capabilities with which beacons are actually endowed. The accuracy of RO-SLAM estimations improves with the number and variety of measurements that are integrated in SLAM. However, using more measurements requires consuming more resources to gather, transmit and integrate the measurements in SLAM, which often contrasts with existing technological or application constraints. This paper presents a scalable robot-beacon distributed RO-SLAM scheme for resource-constrained operation. The objective is to improve SLAM performance while meeting a given resource consumption bound expressed as the maximum number of measurements that can be integrated in SLAM per iteration. In our problem, the number of measurements is a good metric for resource consumption since it directly impacts the sensing channel capacity used, the beacon energy consumed and the computational capacity employed, among others. The proposed scheme efficiently combines a distributed SEIF SLAM method that integrates robot-beacon and inter-beacon measurements, together with a distributed information-driven tool that selects the measurements to be integrated in SLAM balancing uncertainty improvement and resource consumption. The scheme has a robot-beacon distributed approach where beacons actively participate in measurement selection, gathering and integration in SLAM. Our scheme ensures resource-constrained operation with static or dynamic bounds, showing significant flexibility. It achieves higher accuracy and lower beacon initialization times than conventional SLAM methods. Besides, it can be executed in constant time regardless of the map size. Its performance was validated and compared with existing methods in sets of 3D SLAM experiments. Robustness analysis confirmed its stable and predictable performance against transmission errors and different values of its parameters. Acknowledgments: This work has been supported by EU Project AEROARMS Ref. H2020-ICT-2014-1-644271 (https://aeroarms-project.eu/) and the AEROMAIN project funded by the Spanish R&D plan (DPI2014-59383-C2-1-R). A. Torres-González thanks the Ministerio de Educación y Deportes FPU Program. J.R. Martínez-de Dios acknowledges EU Project ARCOW-EuRoC funded under Contract 608849. Author Contributions: A. Torres-González and J.R. Martínez-de Dios contributed equally in the research that leads to this paper. A. Torres-González developed, implemented the algorithms and performed the real experiments and wrote some parts of the manuscript. J.R. Martínez-de Dios helped in the design of the algorithms and elaborated the major part of the manuscript. A. Ollero provided suggestions and corrections during the preparation of the paper. Conflicts of Interest: The authors declare no conflict of interest. Sensors 2017,17, 903 18 of 19 Nomenclature ΣtCovariance matrix of the SLAM global state at time t µtMean of the SLAM global state at time t ΩtUpdated information matrix of the SLAM global state at time t ξtUpdated Information vector of the SLAM global state at time t ¯ Ωt,¯ ξt Predicted information matrix and predicted information vector of the SLAM global state for time t ξi,t,Ωi,tUpdate contribution of beacon bito ξt zr,i,zi,jMeasurement gathered by the robot to beacon bi. Measurement gathered by beacon bito bj hr,i,hi,jObservation models for robot-beacon and inter-beacon measurements Hr,i , Hi,jJacobians of the observation models for robot-beacon and inter-beacon measurements BSr , BSi Sets of the beacons that are currently within the sensing region of the robot and beacon bi, respectively LM List with the number of measurements assigned to each beacon in BSrin measurement distribution MSiSet of measurements gathered by beacon bi NMmax Maximum number of measurements that can be gathered and integrated per SLAM iteration Ji,jUtility function for measurement zi,j ri,j,ci,jReward and cost for measurement zi,j αWeighting factor between reward and cost References 1. Brunner, C.J.; Peynot, T.; Vidal-Calleja, T.A.; Underwood, J.P. Selective Combination of Visual and Thermal Imaging for Resilient Localization in Adverse Conditions: Day and Night, Smoke and Fire. J. Field Robot. 2013,30, 641–666. 2. Banatre, M.; Marron, P.; Ollero, A.; Wolisz, A. Cooperating Embedded Systems and Wireless Sensor Networks; John Wiley & Sons: Hoboken, NJ, USA, 2008. 3. Corke, P.; Hrabar, S.; Peterson, R.; Rus, D.; Saripalli, S.; Sukhatme, G. Deployment and Connectivity Repair of a Sensor Net with a Flying Robot. In Experimental Robotics IX; Springer: Berlin/Heidelberg, Germany, 2006; Volume 21; pp. 333–343. 4. Maza, I.; Caballero, F.; Capitan, J.; Martinez-de Dios, J.R.; Ollero, A. A distributed architecture for a robotic platform with aerial sensor transportation and self-deployment capabilities. J. Field Robot. 2011 ,28, 303–328. 5. Martinez-de Dios, J.; Lferd, K.; de San Bernabe, A.; Nunez, G.; Torres-Gonzalez, A.; Ollero, A. Cooperation Between UAS and Wireless Sensor Networks for Efficient Data Collection in Large Environments. J. Intell. Robot. Syst. 2013,70, 491–508. 6. Cobano, J. A.; Martínez-de Dios, J.R.; Conde, R.; Sánchez-Matamoros, J.M.; Ollero, A. Data retrieving from heterogeneous wireless sensor network nodes using UAVs. J. Intell. Robot. Syst. 2010,60, 133–151. 7. Todd, M.; Mascarenas, D.; Flynn, E.; Rosing, T.; Lee, B.; Musiani, D.; Dasgupta, S.; Kpotufe, S.; Hsu, D.; Gupta, R.; et al. A different approach to sensor networking for SHM: Remote powering and interrogation with unmanned aerial vehicles. In Proceedings of the 6th International Workshop on Structural Health Monitoring (Citeseer), Stanford, CA, USA, 11–13 September 2007. 8. 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. 9. 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), Kobe, Japan, 12–17 May 2009; pp. 8–14. 10. Sun, D.; Kleiner, A.; Wendt, T. Multi-robot Range-Only SLAM by Active Sensor Nodes for Urban Search and Rescue. In RoboCup 2008: Robot Soccer World Cup XII; Iocchi, L., Matsubara, H., Weitzenfeld, A., Zhou, C., Eds.; Springer: Berlin/Heidelberg, Germany, 2009; Volume 5399; pp. 318–330. 11. Nogueira, M.; Sousa, J.; Pereira, F. Cooperative Autonomous Underwater Vehicle localization. In Proceedings of the IEEE Oceans, Sydney, Australia, 24–27 May 2010; pp. 1–9. Sensors 2017,17, 903 19 of 19 12. Djugash, J.; Singh, S.; Kantor, G.; Zhang, W. Range-only SLAM for robots operating cooperatively with sensor networks. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Orlando, FL, USA, 15–19 May 2006; pp. 2078–2084. 13. Torres-González, A.; Martinez-de Dios, J.; Ollero, A. Efficient Robot-Sensor Network Distributed SEIF Range-Only SLAM. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2014), Hong Kong, China, 31 May–7 June 2014; pp. 1319–1326. 14. Menegatti, E.; Danieletto, M.; Mina, M.; Pretto, A.; Bardella, A.; Zanconato, S.; Zanuttigh, P.; Zanella, A. Autonomous discovery, localization and recognition of smart objects through WSN and image features. In Proceedings of the IEEE GLOBECOM 2010; Miami, FL, USA, 6–10 December 2010; pp. 1653–1657. 15. Olson, E.; Leonard, J.; Teller, S. Robust range-only beacon localization. In Proceedings of the IEEE/OES Autonomous Underwater Vehicles, Sebasco, ME, USA, 17–18 June 2004; pp. 66–75. 16. Blanco, J.L.; Fernandez-Madrigal, J.A.; Gonzalez, J. Efficient probabilistic Range-Only SLAM. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS’08, Nice, France, 22–26 September 2008; pp. 1017–1022. 17. Torres-González, A.; Martinez-de Dios, J.; Ollero, A. Integrating Internode Measurements in Sum of Gaussians Range Only SLAM. In ROBOT2013: First Iberian Robotics Conference; Springer International Publishing: Basel, Switzerland, 2014; pp. 473–487. 18. Thrun, S.; Liu, Y.; Koller, D.; Ng, A.Y.; Ghahramani, Z.; Durrant-Whyte, H. Simultaneous Localization and Mapping with Sparse Extended Information Filters. Int. J. Robot. Res. 2004,23, 693–716. 19. Djugash, J.; Singh, S.; Grocholsky, B. Decentralized mapping of robot-aided sensor networks. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2008), Pasadena, CA, USA, 19–23 May 2008; pp. 583–589. 20. Ila, V.; Porta, J.; Andrade-Cetto, J. Information-Based Compact Pose SLAM. IEEE Trans. Robot. 2010 ,26, 78–93. 21. Vidal-Calleja, T.A.; Sanfeliu, A.; Andrade-Cetto, J. Action selection for single-camera SLAM. IEEE Trans. Syst. Man Cybern. Part B Cybern. 2010,40, 1567–1581. 22. Torres-González, A.; Martinez-de Dios, J.R.; Ollero, A. An Adaptive Scheme for Robot Localization and Mapping with Dynamically Configurable Inter-Beacon Range Measurements. Sensors 2014,14, 7684–7710. 23. Thrun, S.; Burgard, W.; Fox, D. Probabilistic Robotics, 3rd ed.; The MIT Press: Cambridge, UK, 2005. 24. Baccour, N.; Koubâa, A.; Mottola, L.; Zúñiga, M.A.; Youssef, H.; Boano, C.A.; Alves, M. Radio link quality estimation in wireless sensor networks: A survey. ACM Trans. Sens. Netw. (TOSN) 2012,8, 34. 25. Stachniss, C.; Grisetti, G.; Burgard, W. Information Gain-based Exploration Using Rao-Blackwellized Particle Filters. Robot. Sci. Syst. 2005,2, 65–72. 26. Boers, Y.; Driessen, H.; Bagchi, A.; Mandal, P. Particle filter based entropy. In Proceedings of the IEEE Conference on Information Fusion (FUSION 2010), Edinburgh, UK, 26–29 July 2010. 27. Heredia, G.; Jiménez-Cano, A.; Sánchez, M.; Llorente, D.; Vega, V.; Braga, J.; Acosta, J.; Ollero, A. Control of a Multirotor Outdoor Aerial Manipulator. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2014), Chicago, IL, USA, 14–18 September 2014. 28. Nanotron nanoPAN 5375. Available online: http://nanotron.com/EN/PR_ic_modules.php (accessed on 6 April 2017). 29. Torres-González, A; Martinez-de Dios, JR; Jiménez-Cano, A; Ollero, A An Efficient Fast-Mapping SLAM Method for UAS Applications Using Only Range Measurements. Unmanned Syst. 2016,4, 155–165. c 2017 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).