key: cord-0061010-frm0ffh2
authors: Ma, Wanfeng; Zhang, Yong; Zhao, Qinjun; Liu, Tongqian
title: Two Particle Filter-Based INS/LiDAR-Integrated Mobile Robot Localization
date: 2020-06-08
journal: Multimedia Technology and Enhanced Learning
DOI: 10.1007/978-3-030-51100-5_31
sha: 5a8bbc3e3c4d3a1dd99fd32b5fc27af1558dfe6d
doc_id: 61010
cord_uid: frm0ffh2

In order to achieve high precision localization, this paper presents an integrated localization scheme employs two particle filters (PFs) for fusing the inertial navigation systems (INS)-based and the light detection and ranging (LiDAR)-based data. A novel data fusion model is designed, which considers the robot position error, velocity error, and the orientation error. Meanwhile, two-PFs based data fusion filer is designed. The position errors measured by the two-PFs in real tests is 0.059 m. The experimental results verify the effectiveness of two-PFs method proposed in reducing the mobile robot’s position error compared with the two-EKF method.

At present day, the mobile robots play a vital role in various fields, such as medical treatment, automatic driving, transportation, warehousing, indoor rescue and so on. These works usually need to be completed in complex indoor environment, and robots have to complete the precise movement, which depends on accurate location information. Consequently, the localization of the mobile robots in indoor environment has gradually become a key research (Wang 2017 , Sherwin 2018 , Xu 2018 .

At present, mobile robot localization is usually achieved by a single method, Global Positioning System (GPS) is a famous example. But for indoor localization, it is difficult to achieve high accuracy positioning, because receiving signal become difficult in indoor environment. Nowdays, indoor navigation method based on local positioning system (LPS) has been proposed (Xu 2017 , Zhang 2015 , Cui 2016 , Cao 2019 . For example, a probabilistic UHF RFID tag localization (Cao 2018) algorithm has been proposed for the robot localization (Jian 2018) . At the same time, the ultra wide band (UWB) system is also used for mobile robot localization.

However, using a single method is difficult to get rid of the inherent disadvantages. For example, to overcome the disadvantage of UWB dependence on additional devices, the no beacon-based approaches have been proposed. The inertial navigation systems (INS) is one famous example (Monica 2018) . However, noted that the INS navigation is poor in long-term navigation. In this case, the light detection and ranging (LiDAR) is used to improve the localization accuracy in long-term navigation. For example, a LiDAR-based localization is proposed (Huang 2016) . Figure 1 shows the automatic driving car using the LiDAR to complete localization. It should be pointed out that although the LiDAR-based localization is accuracy, its processing speed is low. In order to achieve high precision robot localization in indoor environment, and overcome disadvantages of localization using single method. Many researchers began to study the integrated localization scheme. Therefore, the integrated navigation method develop rapidly.

An integrated localization scheme fusing the inertial navigation systems (INS)-based and the light detection and ranging (LiDAR)-based data is proposed in this work. In this model, the INS and the LiDAR measure the robot position in parallel. A novel data fusion model is designed, which considers the robot position error, velocity error, and the orientation error. Meanwhile, two particle filters (PFs) are used as the data fusion filter, one PF estimates the LiDARderived robot position, the other PF fuses the INS-based and the LiDAR-based data. The experimental results have been done to verify that the two-PF method proposed in this work is effective.

The rest of the paper is organized as follows: the INS/LiDAR integrated model for two particle filter is designed in Sect. 2, the proposed two particle filters' performance is investigated in Sect. 3, Sect. 4 give the conclusions. 

The LiDAR-based localization model will be studied in this subsection. To the robot localization, the 2D position (x, y) in east and north directions, velocity V , and the orientation φ are considered in LiDAR-based localization model. The state equation of the PF for the LiDAR-based localization is listed as Eq. 1. ⎡

where T is the sample time, k represents the time index, ω 1 k ∼ N (0, Q 1 ) is the system noise. Using the CPs position, the observation equation is listed as Eq. 2.

where x (i) , y (i) , i = 1, 2, . . . , m is the CP's position, here, the m is the number of the CP, d i,k , i = 1, 2, . . . , m is the distance between the CP and the mobile robot, ν 1 k ∼ N (0, R 1 ) is the measurement noise.

Based on the LiDAR-based localization model, the INS/LiDAR localization model will be investigated in this subsection. The position error (δx k , δy k ) can be computed by the follows

where (δx k , δy k ) is the 2D position error, and (δV x k , δV y k ) is the 2D velocity error. It should be emphasized that the mobile robot's velocity in east and north directions is not easy to be measured. The most common method is to calculate the robot's velocity by using the encoder-derived velocity and the orientation. Thus, we can get the following conclusion.

whereṼ k is the velocity measured by the encoder at the time index k, δV k is the velocity error at the time index k,φ k is the orientation measured by the INS at the time index k, δϕ k is the error of the orientation at the time index k. Thus, Eqs. 3 and 4 can be rewritten as the follows.

For the INS/LiDAR localization, the state equation of the PF can be written as the following. ⎡

where ω 2 k ∼ N (0, Q 2 ) is the system noise. The observation equation can be written as the follows.

where x I k , y I k is the INS position, and x L k , y L k is the LiDAR position in east and north directions, ν 2 k ∼ N (0, R 2 ) is the measurement noise.

In this paper, in order to verify the proposed algorithm's effectiveness, the real test is done. Figure 3 shows the real test environment. The real test was done in the University of Jinan. The test employs one LiDAR, one IMU, and one robot with the encoder. Here, the LiDAR and IMU are fixed on the mobile robot, which are capable of providing the distances between the CPs and the robot, LiDAR-derived position, and INS-based position. 0.02 s is selected as the sample time in this work. It should be emphasized that the test area is small due to the limited detection range of the LiDAR used in the test. Figure 4 shows the mobile robot used in the real test. The position of the corner point, the reference path, and the paths provided by the LiDAR and DR are shown in Fig. 5 . In this figure, we can find that the path estimated by the LiDAR with the corner points' positions is stable. Compared with the LiDAR, the path estimated by the DR is not suitable. 

The two PF's and the two EKF's performances will be investigated in this subsection. The trajectories estimated by the TKF and TPF are showed in Fig. 6 . From the figure, we can see obviously that the TKF has divergence. Compared with the TKF, the TPF is closer to the reference path. The position errors measured by the TKF and TPF are shown in Fig. 7 and Fig. 8 . At the same time, Table 1 shows the difference of the TKF and TPF in the aspect of position error. It is easy to see from the figure that the error of PF is less than that of KF. The position root-mean-square error (RMSE) of two-PF proposed in this work in east direction is 0.073 m, and in north direction is 0.044 m. 

A localization method for INS/LiDAR-integrated mobile robot based on two particle filter is proposed in this work. In this scheme, two PFs are used, one is used for the LiDAR localization, the other one is used for fusing the INS-based and the LiDAR-based data. The experimental results verify the effectiveness of two-PF method proposed in the aspect of reducing the position error compared with the two-EKF method.

IMM-RKF algorithm and its application in integrated navigation system for agricultural robot

A single RF emitter-based indoor navigation method for autonomous service robots

Adaptive robust INS/UWB-integrated human tracking using UFIR filter bank

Uwb-based indoor human localization with time-delayed data using EFIR filtering

Yaw error self-observation algorithm for pedestrian navigation via foot-mounted inertial navigation system

Performance analysis of improved iterated cubature Kalman filter and its application to GNSS/INS

Pole-zero-temperature compensation circuit design and experiment for dual-mass MEMS gyroscope bandwidth expansion

Temperature energy influence compensation for mems vibration gyroscope based on RBF NN-GA-KF method

BFVP: a probabilistic UHF RFID tag localization algorithm using Bayesian filter and a variable power RFID model

Improving UWB-based localization in IoT scenarios with statistical models of distance error

High accuracy navigation information estimation for inertial system using the multi-model EKF fusing Adams explicit formula applied to underwater gliders

Acknowledgment. This article was partially funded by Shandong Key research and Development Program 2019GGXI04026 and 2019GNC106093, was partially supported by Shandong Provincial Natural Science Foundation project ZR2018LF010, was partially supported by the Shandong High school science and technology project under Grant J18KA333.