IROS2019国际学术会议论文集0842

Cooperative Range-only SLAM based on Sum of Gaussian Filter in Dynamic Environments Jung-Hee Kim1and Doik Kim1 AbstractMost range-only simultaneous localization and mapping (RO-SLAM) algorithms have considered direct mea- surements between robot and its neighbor nodes only, even though other inter-node measurements can be actively used to improve the performance. In addition, most of them assume nodes are static, i.e., fi xed on one place and cannot be applied to dynamic environments where some or all of nodes are moving. To address the aforementioned issues, a cooperative dynamic RO-SLAM (CDRO-SLAM) is proposed in this paper, which is a RO-SLAM algorithm based on sum of Gaussian (SoG) fi lter with cooperative measurements in dynamic environments. The proposed CDRO-SLAM integrates all the inter-node measure- ments for localization, which results in a smaller map estimation error with a faster convergence speed than the conventional RO- SLAM. Moreover, the proposed CDRO-SLAM can also track movement of any nodes under the dynamic environment by resetting and updating the SoG variables. Such advantages of the proposed CDRO-SLAM algorithm are verifi ed with the real experimental data. I. INTRODUCTION The problem of simultaneous localization and mapping (SLAM) has been widely researched and applied to many areas, especially in autonomous mobile robot, self-driving cars, rescue, guidance fi elds, etc 1. The solution to the SLAM problem has been proposed in several forms using various sensors, such as visual SLAM 2, bearing-only SLAM 3, range-only SLAM (RO-SLAM) 410, etc. Among them, RO-SLAM which uses range sensors only without bearing information is considered in this paper. There are some unique features in RO-SLAM. Firstly, a complicated data association problem is not included since most practical range sensors are designed to distinguish the unique identifi er of the neighbor devices. This enables the RO-SLAM algorithms to be effi cient and simple enough for wide range of applications such as search and rescue application, and submarine autonomous vehicles. Secondly, range measurements are highly ambiguous since potential location of a node can be anywhere within a ring shape with a radius equals to the measurement with noise. To solve the RO-SLAM problem, the extended Kalman fi l- ter (EKF) has been adopted most widely. With the range-only sensor, the EKF approach has been associated with beacon initialization methods such as trilateration 4, probability grid 5, and particle fi lter 6. Compared with other methods, the particle fi lter approach yields a better estimation accuracy because it can represent an arbitrary probability density *This research was supported by the KIST fl agship program 1Jung-Hee Kim and Doik Kim are with the Center for Intelligent Robotics Research, Robotics and Media Institute, Korea Institute of Science and Technology, Seoul, Koreadoikkimkist.re.kr function (pdf) with a number of the particle under the non- linear system model. However, it requires high computational complexity due to a large number of particles. As an alternative approach, the EKF can be combined with a sum of Gaussian (SoG) fi lter 5, 7. The SoG fi lter needs less computational complexity than the particle fi lter since the required number of Gaussian distribution is much smaller than that of the particle fi lter approach, while it can approximately represent an arbitrary pdf similarly as in the particle fi lter 11. Consequently, the RO-SLAM with the SoG fi lter will give a similar estimation accuracy to the particle fi lter approach, but in a more effi cient way. In order to improve the accuracy of the RO-SLAM, coop- erative algorithms have been introduced. They use not only direct measurements between a robot and its neighbor nodes but also other inter-node measurements for the localization. Despite of such advantages, only a few cooperative RO- SLAM algorithms have been reported 810. In 8, the EKF based RO-SLAM algorithm was fi rstly proposed by simply incorporating the inter-node measurements into the standard EKF framework. In addition, a sparse extended information fi lter (SEIF), designed for effi ciency and scala- bility of the extended information fi lter (EIF), was applied to the RO-SLAM with the inter-node measurements 9. Similar to 8, the cooperative EKF based RO-SLAM, but with the particle fi lter based beacon initialization, was introduced especially for unmanned aircraft systems (UAS) in 10. The cooperative approach can also extend application area of the RO-SLAM to the dynamic environment where nodes attached on some places are moving arbitrarily. With the conventional RO-SLAM, their motion cannot be tracked since ambiguity arisen by moving robot and/or moving nodes cannot be removed with the direct measurements only. On the other hand, with the cooperative approach, it is possible to track such moving nodes over time by utilizing the inter-node measurements. Nonetheless, the conventional cooperative RO-SLAM algorithms 810 assume that nodes are fi xed on some places permanently, and thus, they cannot be applied to dynamically varying situation. In many cases, the place interested is not always static. For example, in the underwater environment such as submarine autonomous vehicle application, actual location of acoustic transponders can be varied within a few meters due to the current of water. In robot-assisted search and rescue application, initial location of pre-installed nodes on the wall or ceiling can be changed after a disaster such as earth- quake, hurricane, fi res, or collapsed buildings. In addition, if movement of nodes attached to moving objects (e.g., doors, 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE2139 -10-50510 x-axis (m) -5 0 5 10 15 y-axis (m) (a) -10-50510 x-axis (m) -5 0 5 10 15 y-axis (m) (b) Fig. 1: Difference between (a) the conventional RO-SLAM and (b) the proposed CDRO-SLAM. Black triangles, green squares, red crosses represent robots, receivers, neighbor nodes, respectively. Black dotted line and magenta dotted line indicate the movement of a robot and measurements of range, respectively. The conventional RO-SLAM estimates the map with a moving robot and direct measurements only; the proposed CDRO-SLAM estimates the map by utilizing the inter-node measurements without moving a robot. robots, people, and others), is tracked in ambient intelligence environments, human activity and interaction with moving objects can be informationized simply and securely because generated data of moving nodes have small data size and it is non-intuitive data which cannot be understood directly by humans unlike vision data. The proposed CDRO-SLAM can track the movement of any nodes by resetting and updating the SoG variables. To the best of our knowledge, the RO-SLAM based on the SoG fi lter with cooperative measurement in dynamic environment is fi rstly proposed in this paper. This paper is organized as follows: section II proposes the CDRO-SLAM including integration framework of inter-node measurements and tracking framework of moving nodes; some experimental results are provided to verify the perfor- mance of the proposed CDRO-SLAM algorithm in section III. Finally, concluding remarks are given in section IV. II. COOPERATIVERO-SLAMBASED ONSOGFILTER A. Overview The proposed CDRO-SLAM is consisted of two stages: initialization and movement. In the initialization stage, the robot and map estimation is guessed by utilizing all the inter-node measurements without moving the robot. In the movement stage, the initial robot and map estimation are further refi ned by moving the robot, or the movement of neighbor nodes are tracked. Some distinct features between the conventional RO- SLAM and the proposed CDRO-SLAM are illustrated in Fig. 1 and summarized as follows: Firstly, the proposed CDRO- SLAM uses all the inter-node measurements for localization while the conventional RO-SLAMs consider direct measure- ments between the robot and its neighbor nodes only. These inter-node measurements give improved convergence speed Algorithm 1: Initialization stage of CDRO-SLAM. (M = number of nodes) /* Iterative multilateration */ for i 0 to M 1 do Initialize location of ithnode by using (1)-(4). end /* SoG generation */ for i,j 0 to M 1, (j 6= i) do Generate SoG variables by using (5)-(7). end /* SoG update */ Update corresponding mk ij and Ck ij to rijusing EKF. for i,j,m 0 to M 1, (j 6= i,m 6= i,m 6= j) do Compute and normalize lk ij,m. Update and normalize vk ij. end /* Merging SoG */ for i 0 to M 1 do Compute new location of ithnode with (12)-(13). end /* Weight transfer */ for i,j 0 to M 1, (j 6= i) do Transfer weight vk ij by using (15). end for i 0 to M 1 do Determine convergence of ithnode. end and accuracy of the map estimation, which will be examined later in the experimental result. Secondly, the proposed CDRO-SLAM initializes the map estimation by using all the inter-node measurements without moving a robot, and refi ned further by moving the robot later. Therefore, it is possible to locate a robot and its neighbor nodes after receiving a few dozens measurements. On the other hand, the conventional RO-SLAM can only estimate the map by moving a robot. Thirdly, the proposed CDRO-SLAM can track the move- ment of neighbor nodes while the conventional RO-SLAM cannot (not explicitly described in Fig. 1). Pseudo code for the initialization stage is given in Al- gorithm 1. The location of each node is fi rst initialized by using the iterative multilateration, and the SoG variables are created at the center of the initialized location for any pair of two nodes, i.e., ithand jthnodes where i 6= j. After updating mean and covariance of the SoG variables by utilizing the standard EKF procedure, the weight of the SoG variables is updated with respect to all possible inter-nodes, whose index is denoted by m in Algorithm 1. Then, the weighted SoG variables are merged as a single Gaussian distribution to estimate new location of each node. Note that in the merging process, the resulting solution can be a local minimum due to the non-convexity inherent in the anchor-free localization. 2140 To overcome such diffi culty, a method to transfer the weight to its neighbor Gaussian distributions is needed. Its detailed explanation will be presented in the following subsection. Finally, the procedures are repeated until the positions of all nodes are converged. In the movement stage, a moving robot refi nes positions of other nodes, or moving nodes are tracked by other nodes and robot. With the proposed CDRO-SLAM, a dynamically changing environment can also be mapped in real-time as far as nodes can be placed at. B. Initialization stage 1) Iterative multilateration: For the initial localization, the iterative multilateration method is employed with a slight modifi cation. In the original iterative multilateration 12, the localization error can be accumulated with each iteration because the solution of the current iteration is affected by not only the distance measurement error but also the position error in the previous iterations. To resolve the problem, the original iterative multilateration approach is slightly modifi ed as follows. If second and third nodes are assumed to be respectively placed in the positive x-axis and in the upper half plane, the positions of fi rst three nodes are set as in the original method 12 as follows: p0= (x0,y0) = (0,0)(1) p1= (x1,y1) = (r01,0)(2) p2= (x2,y2) = ?r2 01+ r202 r212 2x1 , q r2 02 x22 ? .(3) In (1)-(3), rijis the distance measurement between ithand jthnodes. Then, the position of the next node should be estimated by using information of the fi rst two nodes as in the third node not to propagate the position error of the previously localized nodes as follows: pl= (xl,yl) = ?r2 01+ r20l r21l 2x1 , q r2 0l x 2 l ? (4) The sign of ylis chosen as the one whose distance kp2 plk is closer to the measurement r2l. As implied in (4), the solution is affected by only p1in the sense of the position error. If some nodes are not visible to the fi rst two nodes, then they can be similarly localized using two other initialized nodes. 2) SoG generation:Once the initial localization is guessed, the SoG variables, i.e., the mean mk ij, the covariance Ck ij, and the weight vkij, for all pairs of ith and jthnodes, are generated as in the original SoG methods 5, 7. mk ij = ?x i+ rij cos( 2k N ) yi+ rij sin(2k N ) ? ,(5) Ck ij = ?v 1 v2? ?2 s 0 02 t ?vT 1 vT 2 ? ,(6) vk ij = 1 N (7) where k = 0,1,.,N 1, is index of the SoG and N is the number of SoG, respectively. A detailed description of generating SoG variables can be found in Fig. 2. -505 x-axis (m) -5 0 5 y-axis (m) Fig. 2: Generation of SoG variables. Magenta dotted line represents the measurement. v1and v2are the radial and tangential unit vector, respectively. sand tdenote the radial and tangential standard deviation, respectively. 3) SoG update: In this subsection, the update procedure for the weight of the SoG variables is introduced, which is also described in Fig. 1(b). To accelerate and improve the convergence performance of the weight, the proposed CDRO-SLAM updates the weight by exploiting the mea- surements from neighbor nodes. Before updating the weight vk ij, the corresponding mean mk ij and covariance Ck ij of the current measurement rij are fi rstly updated using the standard EKF procedure (see 15). After that, the weight vk ij is updated by computing the likelihood lk ij,m as follows: lk ij,m= 1 22exp (rk ij,m rmj)2 22 ! ,(8) rk ij,m= q (mk ij(0) xm)2+ (mkij(1) ym)2. (9) where is the standard deviation. After normalization of lk ij,m, the weight vkij is updated as vk ij = vk ij l k ij,m. (10) Finally, the weight is normalized as vk ij = vk ij P kv k ij .(11) The procedure is repeated with respect to all possible neigh- bor nodes, whose index is defi ned by m in Algorithm 1 where m 6= i and m 6= j. Note that in (8)-(11), the weight vk ij is updated to a higher value if rk ij,m is close to rmj, and it converges to zero if not. 4) Merging SoG: For each node, several SoG distributions from its neighbor nodes are merged as a single Gaussian to achieve its new refi ned location. The closed-form solution of the existing SoG merging method is adopted here 13, 14. The merged solution (i.e., pi, Ci) is computed in the following closed form: 2141 Fig. 3: The root mean squared error (RMSE) of map esti- mation with and without weight transfer. p i= 1 P j,k ?vk ji ? X j,k ?vk jim k ji ? (12) Ci= N1 X k=0 vk ji(C k ji+ Ck ji), (13) where Ck ji= ?mk ji pi ?mk ji pi ?T .(14) Note that piwill be piin the next iteration. 5) Weight transfer: In updating the weight, it can fall into a local solution due to the non-convexity inherent in the anchor-free localization. To avoid local minimum and to yield a better result with a high probability, a weight transfer method is introduced, i.e., the maximum weight of the SoG distribution is transferred to weights of neighbor Gaussian distributions. The shape of the distributed weight is heuristically chosen as a Gaussian distribution where the center is at the index of the maximum weight, kmax. If dkmaxk= kmkmax ij mk ij k is defi ned, the weight vk ij is distributed as follows: vk ij = 1 22exp d2 kmaxk 22 ! ,(15) where is experimentally determined (e.g., 0.5). Finally, the weight vk ij should be normalized to ensure that the sum is 1. Performance improvement of the proposed weight transfer technique is illustrated in Fig. 3 with a simulation where fi ve nodes were deployed and noise variance was set to 0.30.8. The steady-state RMSE difference was 0.4977m and it shows the effectiveness of the weight transfer technique. 6) Convergence: Convergence of each node can be deter- mined by the time-average of pi. The mean location pi(n) of the ithnode at iteration n is computed with a smoothing parameter as p i(n + 1) = pi(n) + (1 )pi. (16) Here, is experimentally chosen (e.g., 0.90.95 in this paper). If k pi(n + 1) pi(n)k is less than some threshold value, the ithnode is considered to be converged. Otherwise, procedures in sections II-B.3 II-B.5 are repeated when new measurement is available. C. Movement stage There are two possible scenarios in the movement stage: movement of i) a robot and ii) neighbor nodes. When a robot moves, the initial map estimation can be further refi ned. In this case, the standard odometry update and the EKF update are performed, and the procedures are given in 15. A procedure for tracking moving nodes is described in Fig. 4. Initially, we need to detect whether a node is moving or not. For example, if an accelerometer is attached on a node, one of motion detection techniques (e.g., norm of the accelerometer) can be used. Once some moving nodes are detected, they are tracked by using the measurements from the neighbor nodes. If a new measurement from one of neighbor nodes is received, the corresponding SoG variables are reset by using (5)-(7) with the new measurement (see Fig. 4(a). Then, the SoG variables are updated, and they are merged by using (12)-(13) to estimate new location of the moving node (see Fig. 4(b). After that, the SoG variables from the moving node to its neighbor nodes are reset, and the corresponding SoG variables are also updated for the next iteration (see Fig. 4(c). III. EXPERIMENTAL RESULTS Performance of the proposed CDRO-SLAM was verifi ed with real experimental data. For experiments, the ultra-wide- band (UWB) measurements were collected by using the commercial device, Pozyx 16 which has nominal ranging accuracy of 10 cm, maximum range of 100 m, and maximum measurement rate of 80 Hz 17. The used robot in the experiments wa