Non-Uniform Network Mapping and Localization

The purpose of this paper is to establish a new simultaneous localization and mapping (SLAM) framework, which can make better use of the spatial structure of the environment, to improve the robustness and reliability of the system. To locate, a good map should be not only a representation of the real environment, but also a highly compressed and intelligently organized geography information of the environment. Furthermore, the map structure should be stable and robust to inevitable errors. Inspired by human positioning ability, we developed a non-uniform network mapping and localization algorithm (NU-SLAM) based on the complex network theory. Landmarks are classified and sparsely connected according to their localization ability. Therefore, data association and information transmission can be carried out hierarchically with low computational cost and robustness to data association errors. In this paper, a new visual to the mapping and localization problem is proposed. Simulation results show that the algorithm can achieve good results.


I. INTRODUCTION
The SLAM problem has been researched for thirty years, and lots of great results have been achieved. SLAM is a basic ability for a robot to move and navigate in real environment. As the robots are used in more and more fields, the SLAM technology is needed in more and more applications. But the current SLAM methods are still not ready for the real world: (1) Many kinds of features are used to improve the reliability of the map and the data association, and the landmarks in the map are equally concerned and connected. It is not consistent with human experience and has low tolerance for wrong data association. In the real world, the targets are not equally reliable and observable for localization.
(2) The map is a representation of the environment in sparse or dense, features or objects. It is not designed with an intelligent description of the structure of the environment for localization.
(3) It is difficult for traditional maps to intelligently use the structure of the environment for local or global data association, so the localization reliability and the computational complexity are difficult to control.
(4) The information propagates along the trace of the mobile agent. If the trace is highly interlaced, the The associate editor coordinating the review of this manuscript and approving it for publication was Zheng H. Zhu . accumulated errors may cause some problems to the global consistency of the map. It cannot take advantage of the environment structure.
The key to construct a map is to find an efficient and reliable method to describe and organize the environment information. It should have highly organized information of the environment especially for localization, rather than just a complete representation of the environment. In this work we propose a novel SLAM method. The contributions of our SLAM method are: (1) Provide numerical localization capability for each landmark. The localization ability mainly depends on their observable distance.
(2) Construct a non-uniform network map for the environment according to the geometrical relationship and the localization ability of the landmarks. The connection number is restricted with the consideration of the connection complexity and the robust of the map structure. The landmarks with longer observable distance have more connections.
(3) Propose a hierarchical and networked data association and information propagation method for loop closing and global localization. The importance of different landmarks has been effectively utilized.
A virtual environment is constructed by using random landmarks with random localization abilities. Frame to frame transformation is calculated with ICP algorithm [1].
Then a non-uniform network mapping algorithm is proposed to represent the environment. The location of landmarks is first memorized by relative distance, and then absolute map is calculated after the whole map is constructed. When checking the loop or getting some new information, a hierarchical information propagation algorithm will be activated.

II. RELATIVE WORKS A. LANDMARK EXTRACTION
For lasers, landmarks are represented by points with direction and distance, or features with points or lines. For vision, the information is much richer. All kinds of point features, such as SIFT, SURF, ORB etc., lines, regions, objects or keyframes are used as landmarks [2], [3]. The reliability of the landmarks is different in different situations. For localization, the performance of the system will also depends on the localization ability of the landmark, not just its stability. For example, human beings can find a place with a few symbolic objects and ignore some unimportant information. People can evaluate landmarks by their uniqueness, stability, visual range etc.

B. MAP CONSTRUCTION
After the landmarks are observed, they should be organized together to draw a map. The EKF based method was proposed in the early years [4]. In this algorithm, all landmarks are connected with each other. As a result, the connections increase squarely as the landmarks increasing, and the computational cost increases even faster. In this method, the most important thing of map construction is to make a proper trade-off between the complexity of the landmark connection and the consistence of the map. Thrun et al. [5] and Torres et al. [6] replace the covariance matrix in the EKF with sparse information matrix, in which the week connections are ignored. Mazuran et al. [7] uses sparse inverse covariance selection algorithm to obtain higher sparsity. These methods can reduce the connection complexity of the map with some sacrifices of the map consistence. The system is not robust to the wrong landmark association. Recently, Ila et al. [8] uses the information theory, integrating only the informative contribution to the state representation, to make the connection spare, so as to make better use of the environment structure.
Montemerlo et al. [9] proposes Fast-SLAM algorithm, in which the position and pose of the mobile agent are represented by a group of particles. Each of these particles represents a possible trajectory, and the weight of each particle is adjusted by the observation. The system stability is affected by the distribution of the particles. Barkby et al. [10] improves the distribution of the particles. This kind of methods maintain the landmark independence by conditioned on the robot trajectory, so the map complexity can be controlled. But the particle degeneration problem is still troublesome, and the information cannot be spread throughout the map.
In recent years, graph based methods have attracted the most attention [11], [12]. Keyframes are used as graph vertices, and the connections between keyframes are used as edges. Relative bundle adjustment algorithm is used for local or global optimization. In this kind of algorithms, the environment is represented by keyframes, which is more robust to the wrong connection of a single feature. However, the connection chain of keyframes will be very long, and errors will be accumulated. These chains are often interlaced, which makes the chain form a network. This will cause some difficulties to information propagation and global consistence, and at the same time, it will increase the cost of computation. In order to make the graph sparse, many methods [13], [14] have been proposed.

C. DATA ASSOCIATION
Data association includes local and global data association. Its reliability and efficiency depend on the map descriptor and environment structure. Local data association is mainly sequential data association, which requires low time stability and global uniqueness of landmarks. Many constrains are used, such as reliable descriptor [15], motion model [16], chisquare distribution and so on.
Global data association requires that landmarks should be recognized in the whole map after a long journey. Therefore, it is much more difficult, especially when the map is large and has high degree of similarity. Williams et al. [17] uses global unique point features, which can be relocated in a small area. Birem et al. [18] uses salient regions to help data association, which are more stable and unique than single point features. If the environment is large, there will be too many point features. Therefore, Cummins and Newman [19] creates bag of words with SURF features to describe the environment. After a few times of matching and Bayesian inference, the correct position can be obtained. The BRIEF [20] and ORB [21] features are also used to create bag of words to speed up calculations. Latif et al. [22] uses bag of words to detect the loop and Hua et al. [23] adds multi-view geometry information to improve the performance in high similarity environments. Dominik and Giorgio [24] proposes a dynamic self-adaption decision tree to improve the speed of data association, which can be regarded as a heuristic search method.
Currently, researchers are trying to use deep learning methods that can provide semantic information to construct human friendly map and improve the robustness of data association. Bowman et al. [25] uses semantic information for data associations by giving each landmark a class probability.
McCormac et al. [26] drives CNN semantic predictions from multiple view points, and integrates the prediction probabilistically into the map. This can also increase the functional scope of the map.
Global data association is a global search problem, which means big data searching especially in large environment. The above methods improve the reliability of data association to a certain extent by adding constraints to landmarks. However, the environment structure related to the localization ability is little used, which could benefit for the reliability and efficiency of data association. VOLUME 8, 2020

III. SYSTEM DESCRIPTION
We find that the key problems of current SLAM system are: a. the reliability of landmarks is uneven, most of the existing algorithms do not evaluate the localization characteristics of landmarks. b. The map is uniformly structured, so it cannot show the localization specificity of each landmark. c. The data association method cannot take advantage of the environment structure and landmark characteristics, and is not robust to random matching error.

NU-SLAM Algorithm
Input: Observation L and map M Output: Robot location X and map M Initialize the system states The differences of our methods are: a. does not only improve the extraction method of landmarks, but also take advantage of the physical characteristics of landmarks by adding the evaluation of the location characteristics of landmarks. b. Does not only establish new connections depending on the distance between landmarks, but also consider their localization ability to construct a non-unique network map. c. Does not only equally deal with the landmarks in the map to do data association, but also consider the localization ability of landmarks and the map structure, and adopt the method of hierarchical data association. The flowchart is shown in Fig.1.

A. NON-UNIFORM MAP
The map is connected as a network, no matter whether it is built with landmarks or keyframes. If we use landmarks to construct a map, in the existing algorithms, landmarks are fully or sparsely connected with each other, or make a map chain with some kind of tree structure. In recent years, keyframes have been widely used, which will form a map chain. However, geographic map is inherently a network  with specific characteristics: (1) landmarks with strong positioning ability will be more observed and become positioning center, while most landmarks are less concerned because of weak positioning ability. All landmarks will form a map network, and the degree of the network nodes has no obvious specific value, which makes the whole map show scale-free characteristics. (2) In the process of map construction, landmarks are only connected with a certain range of landmarks, which conforms to the local world evolution model. This kind of network structure has strong anti-interference ability to random error according to the complex network theory.
The map should be non-uniform, as shown in Fig.2. The big blue circles represent the most remarkable landmarks. They are more stable and more observable than the other landmarks. The smaller orange circles represent the less remarkable landmarks, and the white circles represent the other landmarks. This map structure can better describe the environment as a map for localization rather than just a reconstruction.

B. OBSERVATION AND TRACKING
For each observation, the rangefinder will get the azimuth and distance of the landmark in the observation coordinate. Each landmark has its own positioning characteristics. Therefore, the state of the landmark is expressed as Li = (r, θ, s), where r is the observation distance, θ is the observation orientation, and s is the strength of the landmark, which reflects its localization ability. After the observation, ICP algorithm is used to match the current observation and the last observation to find out the sensor transformation. If the sensor is kidnaped on a robot and the robot's motion is predictable, then the ICP search space will be greatly reduced. Equation (1) is the energy function of ICP algorithm, p i and q i are the i th feature in two observations. Then the rotation matrix R and the translation matrix T are calculated by LM algorithm.
After the landmarks are matched, they will be connected with each other to form a map. In this work, each landmark has a predefined localization capability, which defines the observation range of the landmark. If the landmark has a long observation range, it will be defined as a strong landmark for localization. After each observation, all currently observed landmarks will be connected to the strongest ones. They follow the following strategies to connect: (1) if there are few number of known landmarks, select the strongest landmark as the starting location. (2) If the landmark is newly observed, connect it to the strongest known observed landmarks, active connection is limited in four, which is a tradeoff of complexity and robustness of the map. (3) If the landmark is re-observed, update the known connection information in the map with CIF filter. Then the connection matrix is constructed: In the equation above, d is the landmark distance, c is the covariance of the distance, t is the observation times of the corresponding landmark.
In equation (5) and (6), r 1 and r 2 are the landmark distances observed by the laser rangefinder, c 1 and c 2 are their variances, θ is the angle between r 1 and r 2 , dθ is the observation error of the angle. They make the landmark distance d 12 and its variance c 12 .

2) INFORMATION FUSION
If the connected landmarks are observed for the second time, the new observations should be integrated into the known connections. Because the observations at different times are uncorrelated, the covariance intersection filter (CIF) is used to fuse the observations at different times.
In the equations (7) and (8), ω is the weight, which is the reciprocal of the corresponding landmark observation time. The lower corner mark f indicates the fusion result, c indicates the current observation, m indicates the known information in the map.

3) MAP FUSION
If there are two or more robots exploring the same place, or a robot exploring the same place on different dates, then we will get several maps of the same place, which should be integrated together. For relative maps, only the relative distances of the landmarks are remembered, and they are independent with each other. Therefore, the connection information can be fused at the same time in the same way as the Information Fusion III-C.2 section above.

D. ABSOLUTE MAPPING
Relative map does not have accumulated error as the robot moves. But it cannot be directly used for localization. Only the relative distances are remembered in the relative map, so we will calculate the location of each landmark in the map with their relative connection information. Inspired by the theory of global positioning system (GPS), the least square method is used to calculate the location of the landmarks in this work. It contains the following steps: (1) select the most connected landmark as the starting point; select the landmark which is connected with the start point and with the most connections as the second starting point; select the landmark which is connected with the selected two points, while not colinear with each other, and with the most connections as the third starting point.
In equation (9), l ix and l iy are the x − y coordinate of the i th landmark, d ij is the distance between the i th landmark and the j th landmark. The coordinates of landmarks j = 1, 2, 3 are known, then two of the three equations can give two initial guesses of the i th landmark's location, and the third equation can choose one,L i org = [l ix , l iy ]. As shown in Fig. 3, the red star is chosen as the starting point, where R i equals to d ji . Let In the equation (11), C ld is the square of the sum of landmark location error and distance error. Then makes L i = L i org + L , iteratively calculate the landmark's coordinates until the error L below the threshold. The i th landmark's location error is: E. GLOBAL LOCALIZATION When a mobile robot wakes up and remembers a map or failed to follow the trajectory, it should know its location in the map, and then it can decide whether to build a new map or update a known map. It should match the current observation to the entire map to find its location. If the map is large, a global search can be time consuming. In human experience, we usually find our location by looking for one or several symbolic targets, and then get the exact location through the local view. Inspired by this, we construct a nonuniform network map. In this map, landmarks are classified into different categories according to their localization ability. Therefore, global data association could be carried out in a hierarchical way like human beings. The landmarks in the map are classified into three levels according to their visible ranges, as shown in Fig. 4. The number in each class is (M 1 , M 2 , M 3 ), with their visible ranges decreases successively. When a new observation is obtained, the landmarks are also classified into three levels, and the numbers are (o 1 , o 2 , o 3 ). The global data association steps are as follows:  If there are n landmarks in the first class of the map and m landmarks in the first class of the observation, the computational complexity is O(mn) by using a hierarchical method of global data association. This can greatly reduce the calculation cost.

IV. EXPERIMENT
We do a simulation to test the effectiveness of the algorithm. The parameters of UTM-30LX laser are used in the simulation, and the Pioneer3-AT parameters are used as the mobile robot. The control noise is 0.05 m/s for speed and pi/60 for angle. The map contains 300 landmarks, and the probability distribution of each landmark intensity is χ 2 , as shown in Fig.5. The intensity is used to determine the farthest observable distance of a landmark.

A. NON-UNIFORM MAP SIMULATION
A non-uniform map is constructed as shown in Fig.6. On the map, landmarks are not uniformly connected. Stronger landmarks are more connected, while week landmarks are less connected. Therefore, some landmarks become the local centers of the map, which is coincident with human experience. Fig.7 shows the map connection matrix. We can see that these  landmarks only connect with their neighbors, which like the FastSLAM [9], SEIF SLAM [6] and TJTF [27] maps in a few years ago, and the current keyframe based SLAM maps. But the different is that each landmark has a different connection number. Therefore, the proposed algorithm will construct a non-uniform map with local center landmarks.
The connection number of each landmark is numerically shown in Fig.8. We can see that most landmarks have only a few connections, while stronger landmarks have more connections, which are prone to become central landmarks. It is different from the known SLAM methods that all the landmarks are equally connected with each other. For example, the EKF based SLAM, in which the landmarks are fully connected with each other. Some SLAM methods reduce the connection number through the connection strength, which is caused by geographical proximity.
The stronger landmarks not only have more connections, but also be more observed, as shown in Fig.9. This is different from the uniform map method, in which landmarks are observed equally. In this algorithm, stronger landmarks are been more observed. This will be conducive to the consistency of the map.

B. SYSTEM PERFORMANCE
For the problem of map construction, operational complexity and map precision are the most important but opposite indicators. Fig.10, 11 and 12 show the calculation time as the map grows. We can see that as the number of the landmark   grows on the map, the calculation time of the traditional EKF-SLAM increases rapidly. The operation time of the proposed method and the FastSLAM method can be maintained at a low level. But the proposed method can be much faster than the FastSLAM.
The location and RMSE errors are given in Table 1. We can see that the location estimation error and the RMSE error of the NU-SLAM is much smaller than that of the EKF-SLAM and FastSLAM. Different from the other SLAM method, in the NU-SLAM method, the most observed landmarks construct a skeleton for the map, which is more reliable and can reduce the estimation error of the whole map.
We also simulated global localization. After building a complete map, the robot will be randomly placed on the trace. Then start a global search, and each time of the global search will take less than 5 ms. This is because the strongest landmarks greatly reduce the search area.

V. CONCLUSION
A Non-uniform network SLAM framework is proposed in this paper. First, the algorithm gives each landmark a numerical localization ability, which is used to distinguish different kinds of landmarks. Then, a novel connection construction method base on the landmark localization ability and its geometrical relationship is developed, and an environmentoriented non-uniform network map is constructed. This map is in keeping with the geographical small world model in the complex network theory. It can give a better tradeoff between the map connection complexity and the structural robustness. Third, the landmarks are relatively connected with each other, and a CIF based hierarchical information propagation method is used to get the global optimization map. This is very convenient for map fusion. Finally, a hierarchical global localization method is adopted, which greatly decreases the global searching time.
The results show that the proposed algorithm is fast while the estimation error is maintained in a low level.
WEI LIU received the Ph.D. degree in pattern recognition and intelligent system from Xi'an Jiaotong University, in 2013. He was a Researcher with the Robotics Institute, Carnegie Mellon University, from 2010 to 2011. He is currently an Assistant Professor with the Xi'an University of Technology. He presided over a national and a provincial natural science funds and participated in a number of national research projects. He has independently completed three enterprise research projects. His research interests include computer vision, intelligent mobile robot, and machine learning.
YANXI YANG received the Ph.D. degree from the Xi'an University of Technology, in 2003. He is currently a Professor and a Doctoral Supervisor with the Xi'an University of Technology. His research interests include complex system control, machine vision, and intelligent robots.
WENQING WANG (Member, IEEE) received the Ph.D. degree in pattern recognition and intelligent system from the Xi'an University of Electronic Science and Technology, in 2015. He has been an Assistant Professor with the Xi'an University of Technology, since 2015. His research interests include machine learning, sparse representation, image fusion, and computer vision of remote sensing images. VOLUME 8, 2020