key: cord-0060953-njj23z0y
authors: Feng, Ning; Zhang, Yong; Xu, Yuan; Bi, Shuhui; Liu, Tongqian
title: LiDAR/DR-Integrated Mobile Robot Localization Employing IMM-EKF/PF Filtering
date: 2020-06-13
journal: Multimedia Technology and Enhanced Learning
DOI: 10.1007/978-3-030-51103-6_26
sha: a6ee7d5104a8f2e08a38d6d78dd52589cd93a91d
doc_id: 60953
cord_uid: njj23z0y

In order to solve the problems that indoor mobile robots have parking during the traveling process and the Extended Kalman filter (EKF) receives too much influence on parameter selection, this paper proposes an Interacting Multiple Model (IMM)-EKF/Particle Filtering (PF) adaptive algorithm for the tightly inertial navigation system (INS)/Light Detection And Ranging (LiDAR) integrated navigation. The EKF and PF calculate the position of the robot respectively, then the smaller Mahalanobis distance-based filter’s output is selected as the initial value of the next iteration, which improves the accuracy of the positioning for the robot. Based on that, the two motion equations of the static and normal motion models are dsigned at the same time. A Markov chain for converting the two state of the model, and the weighting filtering result of the filtered is used to provide distance estimates. The real experimental results show that the IMM-EKF/PF adaptive algorithm improves the positioning accuracy of mobile robots in the presence of parking.

Today, mobile robots play an important role in medical care, home cleaning, indoor rescue, warehousing and transportation. Moreover, mobile robots are often used to serve people in complex environments, so the positioning accuracy of mobile robots is particularly important.

Usually robot positioning is achieved by a single sensor, such as Global Positioning System (GPS), Radio Frequency Identification (RFID), Inertial Navigation System (INS), and so on. But using only a single sensor can't get rid of its inherent drawbacks when used, for example, GPS is the most widely used positioning sensor Wu (2019) . In the case of indoor positioning, it is difficult to complete positioning due to difficulty in receiving positioning signals Wheeler (2018) .

RFID is a wireless communication technology that uses radio signals to identify specific targets and read and write related data. There is no need to identify mechanical or optical contact between the system and a particular target. The cost is relatively low, and the shortcoming anti-interference ability is poor Liu (2006) . INS is an autonomous navigation system that does not rely on external information, but the positioning error of inertial navigation technology accumulates over time, and finally the positioning cannot be completed due to excessive error.

In order to overcome the shortcomings of single navigation technology, many universities and enterprises began to study integrated navigation technology. Therefore, the integrated navigation technology has been rapidly developed Fern (2018). The integrated navigation system can not only make full use of the information of the subsystem, but also complement each other's navigation methods to improve the accuracy of navigation and positioning information Rafatnia (2019) . It is also possible to arbitrarily switch the operating modes of each subsystem to make it flexible and enhance the reliability of the system.

For example, Liu (2006) and Xu (2012) used IEKF for the INS/Wireless Sensors Networks (WSNs)-integrated navigation for indoor robot, from the paper we can see that the EKF is fast for nonlinear systems, but its output is greatly affected by parameter selection.

Integrated navigation used IMM-PF algorithm in Ming (2012) , Particle Filtering (PF) has obvious advantages for the processing of state estimation problems under nonlinear and non-Gaussian conditions Xu (2018) , but because of its large amount of calculation, the real-time performance is poor Sovic (2011) .

In this paper, we proposed a method to combine the advantages of EKF and PF. And then, the interactive multi-model method is used to improve the positioning accuracy of the robot in the stationary/moving situation.

The rest of this article is organized as follows. The second section introduces the INS/LiDAR tight combination model structure based on the EKF/PF adaptive algorithm. Besides section introduces the IMM algorithm under two different models of stationary motion model and moving motion model. A Markov chain for converting the two state of the model, and the weighting filtering result of the filtered is used to provide distance estimates. The third chapter mainly introduces the experimental environment and experimental results. Finally, fourth chapter gives the conclusion.

In this section, we will introduce the INS/LiDAR tight combination model and the adaptive method of EKF/PF, and then, the model selection of IMM will be investigated. Figure 1 shows integrated navigation strategy combining INS and LiDAR measurement.

In this model, firstly, INS and LiDAR separately calculate the distance from the feature point to the robot, in this paper, the feature points use the corner points extracted by the radar from the environmental information, and (d Lidar 

In this section, we will introduce the state equation and observation equation of EKF/PF adaptive algorithm used in this article, and how to select EKF or PF filter will be designed. Equation 1 is the state equation of the PF and EKF for the INS/LiDARbased localization model. ⎡

where the k means the time index, δx k and δy k mean the robot's position error in east and north directions, V means the robot velocity error, ϕ means the heading angle error, T represents the sample time, ω k is the system noise. The observation equation is listed as Eq. 2. 

where v k is the observed noise matrix of the system, and its covariance matrix is R.

In order to solve the situation that the mobile robot has a pause in the indoor motion state, we use the IMM algorithm to deal with it. Figure 2 shows the flow chart of IMM-EKF/PF Algorithm. Firstly, the optimal error estimate δPo M EKF , δPo M P F is calculated by EKF and PF under the moving motion model. The Mahalanobis distance of the two values D M P F , D M EKF is calculated, and the value with a smaller Mahalanobis distance is selected as the output of the filter. Meanwhile, the stationary motion model δPo S can be calculated in a similar way.

Finally, the optimal error estimate δPo S , δPo M are calculated in two states for output interaction, and the optimal error estimate δPo IMM including the motion model and the stationary model is obtained. Through the above steps, we get the optimal position error estimate calculated by the EKF/PF adaptive filter under stationary motion model and moving stationary motion model states.

For each ith, the Markov transition probability matrix has been listed as follow.

where π is the conversion probability of the filter. The weight matrix of each motion model is used as follows.

The moving motion model state equation is listed as follows. ⎡

The stationary motion model state equation is listed as follows.

The observation equations of the stationary motion model and the moving motion model are consistent with Eq. 2.

To verify the validity of the IMM-EKF/PF algorithm, we did a real experiment in the 1st Teaching Building, University of Jinan. The real experimental environment is shown in Fig. 3 .

This experiment mainly uses the following equipment, one LiDAR, one INS, one mobile robot and one computer. The LiDAR and INS are fixed on the mobile robot, the computer is used to collect the data returned by the sensor. In order to synchronize the sampling time of the LiDAR rotation, the INS sampling time T is set to 0.15 s, same with LiDAR. In addition, the IMM-EKF/PF algorithm initial parameters are as follows

and 

The model of sensor and mobile robot combination is shown in Fig. 4 . From Fig. 5 we can see four tracks, the black dotted line is the robot's preset reference track. The solid green line is the trajectory calculated using only the LiDAR to the corner point distance least squares method, the solid blue line is the robot position calculated by INS only, and the red solid line is the trajectory calculated by the tight combination EKF, the parameters Q = 10 −2 × I, R = 10 −2 × I. It can be seen from the figure that in the case where the position of the robot is estimated using only the INS, the error of the INS gradually diverges with time, and the positioning cannot be caculated. So we can know that INS is not suitable for positioning alone. Compared with INS, LiDAR uses the position of the corner points to calculate the position of the robot much more accurately but the accuracy of the individual points is poor. Tightly combined EKF and the least squares method calculate the position, the burr is less, the precision is higher.

Since the EKF is greatly affected by the parameters and the PF is less affected by the parameter selection, we combine the two filters, and select the output of the double filters with a smaller Mahalanobis distance as the initial value of the next iteration.

Under parameters Q = 10 0 × I, R = 10 −2 × I, the sampling point of the PF is 6000, the EKF, and EKF/PF adaptive algorithm effects are shown in Fig. 6. In Fig. 6 , we can see the EKF employs the parameters Q = 10 0 × I, R = 10 −2 × I, performance is significantly worse than EKF under parameters Table 1 . Figure 7 shows the EKF/PF adaptive algorithm and the EKF algorithm with effect of the mobile robot in the presence of parking conditions. The red line is the robot trajectory calculated by the IMM-EKF/PF algorithm, and the blue line is the trajectory calculated by the EKF alone, two trajectories are closer. And Table 2 lists the localization error between EKF and IMMM-EKF/PF. From the Fig. 7 and the Table 2 , it can be seen that the performance of the proposed IMM method is smaller than the EKF tight combination method and the precision is higher. 

The algorithm in this paper is based on a tightly coupled integrated positioning strategy of INS/LiDAR mobile robot. The integrated navigation strategy effectively solves the problem of indoor robot accumulating errors over time using a single sensor INS. In this paper, an adaptive filter of EKF and PF is proposed to solve the problem that EKF is greatly affected by parameter selection.

Then based on this, an adaptive algorithm based on IMM-EKF/PF is proposed to solve the phenomenon that indoor robot have parking during the traveling process. The effectiveness of the adaptive algorithm of IMM-EKF/PF is verified by a real experiment to simulate the operation of indoor mobile robot. In the filter operation phase, there are two EKF/PF hybrid filters, representing two motion models of the robot, one is the normal motion model and the other is the stationary motion model. A Markov chain for converting the two state of the model, and the weighting filtering result of the filtered is used to provide distance estimates. Compared with the traditional EKF tight combination algorithm, the adaptive algorithm of IMM-EKF/PF obtains better robot positioning accuracy when the indoor robot has the moving motion and stationary motion double model.

INS/magnetometer integrated positioning based on neural network for bridging long-time GPS outages

Relative navigation: a keyframe-based approach for observable GPS-degraded navigation

ATENEA : Advanced techniques for deeply integrated GNSS/INS/LiDAR navigation. Satellite Navigation Technologies European Workshop on

Fuzzy-adaptive constrained data fusion algorithm for indirect centralized integrated SINS/GNSS navigation system

Algorithm of imm combining kalman and particle filter for maneuvering target tracking

Unbiased tightly-coupled INS/WSN integrated navigation based on extended Kalman filter

Inertial/geomagnetic integrated navigation algorithm based IMM-PF

Particle filtering for indoor RFID tag tracking. Statistical Signal Processing Workshop

Particle filtering for indoor RFID tag tracking