A Closed Loop Detection Method for Lidar Simultaneous Localization and Mapping with Light Calibration Information

In this work, we study the loop detection problem for mobile robots in the process of simultaneous localization and mapping. In a complex environment, the accuracy and recall rate of loop detection will decrease significantly because of the lack of prominent environmental state information. The proposed method combines light fidelity (Li-Fi), used for sensing the position of mobile robots, with laser ranging technology to recognize the location that the robot has been aware of during the period of locating and mapping. A shortest link processing mechanism for Li-Fi calibration is proposed to select data, thus smoothing the system. Using an extended Kalman filter, we construct virtual consistent state components of Li-Fi information. Scans are enriched by providing more environmental information in this method. A distanceregion model is designed to reduce the amount of calculation appropriately in the process of scan matching. The transformation between frames is reconstructed using a weight distribution from the data of laser and photoelectric sensors to reduce the error of data association. To reduce the impact of Li-Fi link occlusion, we set up a pause mechanism during the period of fusing information in the process of scan-to-map matching. Results of experiments in corridors and offices show that the proposed method can detect key frames, effectively suppress pose drift, improve the accuracy, and guarantee a satisfactory recall rate of loop closure detection.


Introduction
In robotic localization, as mobile robots explore the environment, they use sensors to acquire information about the state of the environment, so as to model the environment and build and update maps. (1) This process, named simultaneous localization and mapping (SLAM), is the foundation and key in the field of robotics to solve the tasks of exploration, detection, positioning, and navigation in different environments. Depending on the sensors used, SLAM is divided into laser SLAM and visual SLAM. Two-dimensional (2D) laser SLAM, which is divided into filter-based and graph-based optimization SLAM (2,3) depending on the solution method, is more suitable than visual SLAM for indoor mobile robots owing to its relatively mature algorithms for localization, mapping, and navigation.
The task of laser SLAM is to estimate the position and pose of the main body of the robot in motion, and to build a map of the surrounding environment. Accurate mapping requires accurate positioning, which in turn requires an accurate map. (4) During the period of loop closure detection in SLAM, the mobile robot judges whether its current position has been described in the created map from the sensor information, which involves global data association. Loop closure detection is such an important task that the correct closed-loop information can be used to modify the cumulative error of an odometer and to solve the problem of pose drift. Incorrect loop closure detection will not only affect the accuracy of the results, but may also lead to the failure of convergence or converge to incorrect results in optimization. (5) Meanwhile, it is difficult to use loop closure detection when mapping an unknown environment, particularly in the following indoor environments: • Degraded environments where the data association error increases owing to the lack of available environmental information, such as a long corridor with no obvious feature points, which aggravates the difficulty of loop closure detection. • Complex environments with many similar objects such as tables, chairs, and corners. This is because similar observations do not necessarily come from the same scene, which may cause perceptual ambiguity. When estimating the probability of observations coming from the same place, the amount of data to be processed will multiply increasingly, which increases the difficulty of building a detailed map of such an environment. To solve the above problems, mobile robots are required to sense more reliable and accurate environmental information. In fact, the surroundings of a robot are unpredictable, but the degree of uncertainty is in a certain range. In addition, the amount of information perceived by sensors is limited. Therefore, from the data of a single sensor, it is difficult for a mobile robot to clearly sense its exact location and the surrounding environment, especially in the above listed environments. In this regard, many scholars have studied the SLAM method based on multisensor fusion. Castellanos et al. (6) proposed a multisensor fusion scheme that emphasized the idea of individual compatibility and joint compatibility of measurement data at the landmark level, but the selection of landmarks was closely related to environmental characteristics. Vasconcelos et al. (7) presented a fusion method involving a camera and a laser ranging sensor, and studied the method of parameter calibration between the two sensors, although the method required a large amount of calculation.
In recent years, LEDs have been widely used in lighting because of their long life, high brightness, and fast response. Light fidelity (Li-Fi) is a hot communication technology based on LEDs, which achieves communication by controlling the fast switching of LEDs. An indoor positioning system based on Li-Fi can achieve high-precision positioning indoors without electromagnetic wave communication. Many papers published worldwide have reported indoor positioning technology based on Li-Fi. (8,9) An indoor visible light positioning system based on the received signal strength (RSS) has low complexity, is easy to implement, has a low cost, and can locate the position of a robot. (10) Combined with laser SLAM, such a system can reduce the occurrence of perceptual ambiguity and the amount of data association. This paper presents a system combining Li-Fi with 2D lidar SLAM for loop detection in real time based on indoor light calibration. To our knowledge, this system is one of the few applications fusing Li-Fi-based calibration with SLAM for mobile robots. The main contributions of this paper are as follows: • A shortest link processing mechanism for Li-Fi information based on the receiver, which is of vital importance to our method, is proposed to select a certain amount of data and smooth the system. The number of links is determined from the received signal through experiments. • A probability model of the Li-Fi-based measurement information is established to make the calibration more accurate, and an extended Kalman filter (EKF) is used to construct virtual consistent state components. The Li-Fi-based information is used as particular criteria in the loop closure detection. In the iterative solution of data association, we use weights proportional to the probability to reconstruct the robot pose. • We report a distance-region model based on scan matching to accelerate the sequential and global association, which guarantees that our algorithm performs with no extra calculation while fusing the information based on laser and photoelectric sensors.

Li-Fi Positioning Model Based on White LED
The indoor positioning system based on white LED communication technology consists of LEDs with transmitters installed on the ceiling and a receiver. Let the coded information of the LEDs at the transmitter be , which is given according to the inherent size of the environmental structure. In the process of positioning, the transmitter transmits coding information i x T through each LED, and the receiver obtains reference information through photoelectric conversion and intensity detection technology. Owing to the relatively small size of LEDs compared with the transmission distance of actual light signals, LEDs are often analyzed as Lambertian. Some existing indoor visible light communication systems based on RSS only consider direct links. As a result, the positioning accuracy of the algorithm is 3-10 cm after simple correction. (10) In this paper, an RSS positioning algorithm is used to increase the real-time performance in SLAM despite its low complexity.
Let the emission angle of an LED be α, the distance between the LED and a photodiode be d i , and the angle between a direct light signal and the photodiode normal be β. The direct channel model describes the optical signal transmission characteristics between the transmitter and the receiver, and its transmission function i where A is the effective photosensitive area of the photodiode, T s (β) is the gain of the optical filter, G(β) is the gain of the condensing lens, and m t and m r are the Lambert constants. Let the power of the light signal received by the receiver be P r and the radiation power of a single LED be i t P . The relationship between the received power and the radiation power without considering the multipath effect can be expressed as To reduce the negative impact of the intersymbol interference in visible light communication caused by multiple indoor reference points, it is necessary to introduce code division multiple access (CDMA) (11) technology to recover light intensity attenuation information and the ID of different LEDs from the overlapped signal. Through Eqs. (1) and (2), we can deduce the distance between each LED and the receiver. The equation is as follows:

Probabilistic Model and Notation
In this paper, the error δ t is regarded as Gaussian white noise, and δ t has a multivariate Gaussian distribution with mean 0 and variance Q t . The EKF assumes that the measurement probability is controlled by the nonlinear function h(P r , x t ) (Li-Fi measurement function): By linearizing h(P r ) around the current state ˆt x using a first-order Taylor approximation, the measurement probability p(z t,1 |x t ) based on Li-Fi has the following expression: . The matrices J t are the Jacobians of the function h evaluated at the current state ˆt x , Suppose that S ref, i = {p n , n = 1, ..., N j }, also named the reference frame, is a set of N j point measurements acquired at a previous sensor location R ref , and S new, i = {q m , m = 1, ..., M j }, named the target frame, is another set of M j point measurements acquired at the current sensor location R new (p n , q m ∈ R 2 ). Let the homogeneous matrix T j = (x j , y j , θ j ) ∈ R 2 × [0, 2π] ( j = 1, 2) be the relative transformation between two adjacent frames. The goal is to register the two frames of point cloud data or the current scanning point with the established map by computing the coordinate transformation relation T j . (12) The calculation method of the laser measurement probability is as follows: when querying, the laser sensor generates a series of measurement values, (1) and K is used to express the number of measurement values in a measurement z t , namely, where K t z is used to represent an independent measurement (a ranging value). Owing to the mutual independence between the measured values, the probability of measurement

System architecture
In Sects. 2 and 3, we respectively described the shortest link processing mechanism and the probability model based on Li-Fi. These modules are the foundation for the data processing in lidar SLAM based on Li-Fi. Our aim is to use these measures to improve the system, whose whole process is shown in Fig. 1. Through the sensors, a laser, and a photoelectric sensor, mobile robots sense point measurements from their surroundings, and the information acquired from the photoelectric sensor is calibrated. The uncertainty of the system is calculated using the probabilistic model. The rest of this system provides two key components. First, an algorithm is used for the scan matching of two contiguous frames to establish data correspondence and to compute the robot pose according to the probability estimation. A distance-region model is proposed to accelerate this process. Second, a scan-to-map method based on Li-Fi calibration is presented for matching between the current frame and a map. The calibration serves as a sign of loop closure in the process, which is determined by the closing mechanism. In the following, we discuss the process of scan matching and the scan-to-map method, covering the distance-region model and the pause mechanism in detail.

Scan matching of two adjacent frames
The quality signal Q returned by the lidar reflects the reliability of ranging results. When the actual measurement distance of the lidar exceeds a certain value L, the quality Q will decline, and the ranging information will become basically unreliable. To reduce the measurement error, it is necessary to filter the cloud data of each frame and discard the lowquality data points. This process can be carried out by our method based on the distance-region model.
The iterative closest point (ICP) is a classical method of spatial point registration. (13) In this paper, the ICP framework is used to match 2D Euclidean spatial point sets. In accordance with the general framework, our method performs the following two steps to register the point set through iteration, so as to preprocess the data for loop detection.
Step 1: Establishing data correspondence between frames The correspondence between data sets of frames is established by the nearest-neighbor principle, and the general method of solving this problem is to compare the data one by one, with the time complexity relatively high. In addition, the light calibration via Li-Fi can probably be fused without increasing the calculation burden. We propose the distance-region model shown in Fig. 2 to speed up the search of the nearest point and the data preprocessing in the loop detection, and to improve the real-time mapping performance of laser SLAM when the information dimension of the data frame increases, thus smoothing the system. 1) By registering data points based on Li-Fi information, the Li-Fi-based transformation matrix T 1 is computed. Since the light source information is calibrated manually, the projection information of the light source is relatively accurate. In addition, the robot receives the same signal from the same light source, and the errors caused by measurement, signal processing, and calculation are reflected in the Li-Fi probability model. As a result, the homogeneous transformation of the Li-Fi data frame based on the measurement probability is accurate, and the Li-Fi-based transformation matrix T 1 is solved using Eq. (13). 2) Using the matrix T 1 , the current laser point cloud data is mapped and transformed to obtain the virtual frame, and the data in the virtual frame is preprocessed by a spatially adjacent sequence: where the function f transforms the data points in S new,i to the reference frame coordinate system through the homogeneous transformation between frames, In Fig. 2(b), the first row represents inv new S and the second row represents inv fus S . When solving the nearest-neighbor relationship, we can search from the current data center to both ends of the data axis using the distance-region model.
Step 2: Solving the relative transformation By considering the correspondence established in the previous step, the objective is to compute the homogeneous transformation that makes the corresponding points match to the maximum extent. The sum of the squares of the distances between the corresponding points is minimized by changing the transformation matrix, whose formal description is  Since laser-based and Li-Fi-based point cloud data are measured by different sensors, it is difficult to achieve their optimal registration at the same time. Therefore, we first use Li-Fi-based data point registration to obtain T 1 , and then use T 1 to construct a virtual frame to accelerate the solution of the relative transformation T 2 of laser-based point cloud data. To balance the laser-based point cloud information and Li-Fi-based information, we use the probability estimation of the measurement model to distribute the weights of T 1 and T 2 , and then reconstruct the transformation T k between frames, ( ) where k is the iteration number and w prop is the probabilities-based weight coefficient expressed as

Matching of current frame to map
A submap is built according to the matching between the current frame and the map. Figure  1 depicts the whole process. Map updating meets the characteristics of a low frequency and realtime. Therefore, the specific process of matching between the current frame and the map is as follows: (1) Transform the current frame fusion data from the body coordinate system to the world coordinate system. (2) Call the set of the current frame S new,i and carry out the intersection operation with the position set SM of the map. If S new,i ∩ SM = S new,i and the number of elements in the set is not less than Link n (which means that the Li-Fi link of the current frame is unobstructed; otherwise, it means that the intersection operation is meaningless and the registration should only be based on the laser point cloud data), then implement step (3). Otherwise, carry out the step of matching between the next frame and the map. (3) Establish the data correspondence between the current frame and the map, then solve the relative transformation. In this process, if the sum of the squares of the distances between corresponding points is less than a threshold, the current frame will be defined as the looping frame.

Evaluation
In this section, we outline the evaluation results. We tested the proposed method with a mobile robot running autonomously on real-word data.
The method was evaluated with a Komodo robot [ Fig. 3(a)] equipped with a 180° HOKUYO laser range finder and a photoelectric sensor [ Fig. 3(b)] within our university [ Fig. 3(c)]. We replaced the lights on the ceiling with LEDs with transmitters. Some relevant parameters of the sensors and LEDs are depicted in Table 1. The environment, a symmetrical room with a corridor, was 10 × 10 m 2 with a height of 3 m. In addition, there were four similar columns and some areas on one side of the room are transparent, and the corridor was very long with a polished floor.
The first experiment evaluated the effect of the number of links of the communication on the positioning error. While changing the number of effective links within the environment, the positioning error was measured and the result is shown in Fig. 4. We can see that four links give the smallest position error based on light calibration in the experimental environment. Therefore, in the second experiment, we set Link n , i.e., the number of links, to four.
The second experiment evaluated the global mapping performance of our method using the Komodo robot. The robot traveled about 160 m around the environment with Li-Fi-based SLAM, as shown in Fig. 5(a). The environment is complicated for the mobile robot because it is axisymmetric and there were four similar columns in the room with no reflective structure in some areas. Figure 5 depicts the results obtained with raw odometry, our method (Li-Fi-based SLAM), and the pIC method (14) (a probabilistic model similar to our method). We can see that the visual result of Li-Fi-based SLAM is better than that of pIC, since it can correct the odometer error with the light calibration. In particular, it aligns the map where there is no frontal structure in   the room. In these areas, the lidar has a modest effect and little data can be collected by itself. According to the results, there is no extra calculation burden for fusing information. However, the mapping performance shown in Fig. 5(c), which is marked by a dotted circle, is slightly lower than that in Fig. 5(d). This is because the light intensity may change with time. On the whole, our method can effectively suppress pose drift. By marking the starting point and the driving path, we let the robot run with Cartographer (2) (a main closed-loop detection method in 2D laser SLAM), and our approach starts from the same point every time and then returns to the starting point. In this way, we obtain many results of our approach. Figure 6 shows the precision and recall ratios of Li-Fi-based SLAM and Cartographer. With the increase in the number of judgment conditions of loop closure detection, the recall rate decreases. (15) In our method, when the precision is the same, the recall rate from Li-Fi-based SLAM is similar or even slightly better than that of Cartographer. This is because the pause mechanism can abandon Li-Fi information when the link appears to be covered. At this time, the information collected from the lidar is enough. The shortest link processing mechanism enables the system to collect only partial data from Li-Fi. Thus, our method guarantees a satisfactory recall rate of loop closure detection while fusing the Li-Fi calibration. Our method has a reliable comprehensive performance for a closed-loop detection system.

Conclusion and Future Work
We presented a laser SLAM loop detection method based on indoor light calibration to optimize a map by optimizing the robot pose. In our method, we assume that the intensity of indoor light does not change with time. Our contribution is to fuse light calibration information during the data frame transformation, and we propose a distance-region model to speed up the process of nearest-neighbor searching. In the process, we establish a Li-Fi probabilistic model to describe the link connection error. In addition, the shortest link processing mechanism and the pause mechanism increase the comprehensive performance of our closed-loop detection system.
In future work, we will concentrate on an adaptive algorithm for the data acquisition of light links while mapping to reduce the link connection error based on Li-Fi positioning, thus providing the system with the shortest link processing. By this method, the number of links will be finalized and the distance information relative to the sensor based on the light calibration information received by the robot will be more accurate, even in different environments.

About the Authors
Qiang Wang received his Ph.D. degree in control science and engineering from Beijing Institute of Technology in 2014. He is an associate professor in the School of Logistics Engineering of Wuhan University of Technology. His research interests are multi-agent system cooperative control, nonlinear system control, and intelligent control and optimization.
Yong Zeng expects to obtain his B.S. degree in engineering from the School of Logistics Engineering of Wuhan University of Technology in 2021. His major is mechanical design, manufacturing, and automation. His research interests are sensors, robots, and control science.
Yin-Kang Zou expects to obtain his B.S. degree in engineering from the School of Logistics Engineering of Wuhan University of Technology in 2021. His major is mechanical design, manufacturing, and automation. His research interests are simultaneous localization and mapping, computer vision, and semantic map construction.
Quan-Zhen Li expects to obtain his B.S. degree in engineering from the School of Logistics Engineering of Wuhan University of Technology in 2021. His major is mechanical design, manufacturing, and automation. His research interests are sensors, robots, and computer vision.