Computer Modeling in Engineering & Sciences

DOI: 10.32604/cmes.2022.017533

ARTICLE

Range-Only UWB SLAM for Indoor Robot Localization Employing Multi-Interval EFIR Rauch-Tung-Striebel Smoother

Yanli Gao1, Wanfeng Ma2, Jing Cao2, Jianling Qu1 and Yuan Xu2,3,*

1Qingdao Branch of Naval Aviation University, Qingdao, 266000, China
2School of Electrical Engineering, University of Jinan, Jinan, 250022, China
3Shandong Beiming Medical Technology Co., Ltd., Jinan, 250022, China
*Corresponding Author: Yuan Xu. Email: xy_abric@126.com
Received: 18 May 2021; Accepted: 11 August 2021

1  Introduction

In recent years, the mobile robots have been widely used in the medical field [1,2]. Especially affected by the COVID-19 more and more jobs in hospitals require mobile robots to replace doctors, which provides the demand for indoor localization of mobile robots in indoor environment. However, there are challenges in the research of mobile robot localization for indoor environment [3,4].

To the mobile robot localization, the global navigation satellite system (GNSS) has been proposed and widely used [5,6]. For instance, in [7], the global positioning system (GPS)-based approach has been widely used in the small mobile robot. the GPS has been used to assist the simultaneous localization and mapping (SLAM) for mobile robot [8]. In [9], the GPS/BeiDou integrated navigation satellite system (BDS) has been proposed. It should be emphasized that although localization method based on the GNSS can provide the position information, this method still have shortcomings:

•   The localization accuracy is meter-lever, which is not suitable for indoor localization.

•   In the indoor environment, the GNSS signal may be outage.

In order to overcome these shortcomings, there are many attempts. For instance, some existing research proposes to use radio frequency identification based scheme (RFID) which has been investigated for indoor robot localization [10,11]. Moreover, the RFID is also been used in moving object localization [12]. A new method of RFID system has been developed for localizing mobile robot employing configures reader (antenna) [13]. In [14], the WiFi has been used in mobile robot positioning. Noted that both the RFID and the WiFi are able to provide the position information in indoor environment, the localization accuracy of the RFID and the WiFi is not suitable for the high precision indoor mobile robot localization [15]. In order to provide more accurate localization information, the localization scheme employing ultra wide band (UWB) has been proposed [16]. For example, in [17] and [16], the UWB is used to measure the mobile robot's position. It should be emphasized that the localization scheme using UWB improves the positioning accuracy.

Based on the localization method, the data fusion method is investigated. For example, the Kalman filter (KF) is used to fuse the data of the laser range finder (LRF) sensor for the mobile robot [18,19]. In order to face the nonlinear system, the extended KF (EKF) filter is proposed for the indoor localization of a mini unmanned air vehicle (UAV) based on low-cost inertial measurement unit (IMU)/UWB/Vision [20]. The improved cubature Kalman filter for GNSS/INS using transformation of posterior sigma-points error is investigated [21]. In [22], the vehicular INS/GPS-integration system based on the unscented KF UKF is derived. The cubature KF is designed for GNSS/INS under GNSS-challenged environment in [23]. One can infer that the Kalman filter is sensitive to the noise initial value and noise statistics, which is hard to obtain in real-time localization [18]. In order to improve the robustness of the localization, the unbiased finite impulse response (UFIR) filter is proposed [24]. For example, in [25], an adaptive-horizon iterative UFIR filter is derived. the FIR filter is employed for the RFID localization [26]. Moreover, in order to improve the localization accuracy, the Rauch-Tung-Striebel (R-T-S) smoother is proposed. for example, in [27], the ensemble Kalman smoother is designed for indoor mobile robot.

For improving the localization accuracy, this work proposes a multi-interval extended finite impulse response (EFIR) R-T-S smoother for the indoor robot's range-only ultra wide band (UWB) simultaneous localization and mapping (SLAM), which employs EFIR filter as the forward filter and the R-T-S smoothing method to smooth the EFIR filter's estimation of the east position, north position, and the UWB Reference Node (RN)'s position, respectively.

The contribution of this work resides in the following:

•   A range-only UWB SLAM scheme is designed for indoor robot.

•   A new ERTS smoothing algorithm employs the EFIR filter as the forward filtering and the R-T-S smoothing as the backward smoothing method is proposed.

•   A new smoothing scheme is proposed for the estimation of the east position, north position, and the UWB RN's position, respectively.

2  The Data Fusion Model for Range-Only UWB SLAM for Robot Localization

In this section, the robot localization scheme for fusing the range-only UWB measurements will be designed firstly. Then, the data fusion model based on the range-only UWB SLAM scheme will be investigated.

2.1 The Scheme for Fusing the Range-Only UWB Measurements

Fig. 1 displays the structure of the scheme for fusing the range-only UWB measurements. In this work, the UWB localization system includes 4 reference nodes (RNs) and one blind node (BN). The RNs are pre-positioned on static coordinates, and the BN is fixed on the robot. When the UWB localization system works, BN is used to collect the ranges diU,i∈[1,4] from ith RN to BN. Then, the ranges diU,i∈[1,4] are input to the multi-interval EFIR Rauch-Tung-Striebel (ERTS) smoother, which used as the data fusion filter in this work. Finally, the multi-interval ERTSS provides the mobile robot's position P^o.

Figure 1: Range-only UWB measurements fusion structure

2.2 The Localization Model for the Data Fusion Filter

In this subsection, the localization model for the data fusion filter for the model mentioned in Section 2.1 will be investigated. The state equation can be written as Eq. (1).

[X^l|l−1RPo^l|l−1RN]⏟X^l|l−1=[A4×4R04×808×4I8×8]⏟AX^l−1+wl−1,(1)

where the time index is expressed in l, wl∼N(0,Q) means the noise in system, the vector of the UWB RNs’ position is shown as the follows:

PolRN=[x1,y1,x2,y2,…,x4,y4]T,(2)

where (xi,yi),i∈[1,4] denotes the ith UWB RN's position, the vector of the mobile robot is shown as the follows:

X^lR=[xlylVxlVyl],(3)

where the items xl, yl,Vxl,Vyl is the robot's position in east directions, the robot's position in north, the robot's velocity in east and the robot's velocity in north directions at the index l for time, the A4×4R in Eq. (1) is listed in Eq. (4).

A4×4R=[10Δl0010Δl00100001],(4)

where the Δl is the sampling interval.

The measurement equation for the data fusion filter is listed in Eq. (5).

[d1,lUd2,lUd3,lUd4,lU]⏟Yl=[(xl−x1)2+(yl−y1)2(xl−x2)2+(yl−y2)2(xl−x3)2+(yl−y3)2(xl−x4)2+(yl−y4)2]⏟h(X^l|l−1)+vl,(5)

where the di,lU,i∈[1,4] is range from the ith UWB RN to the mobile robot at the time index l, vl∼N(0,R) is the measurement noise.

3  The Multi-Interval EFIR Rauch-Tung-Striebel Smoother

In this section, the multi-interval ERTS smoother will be designed based on the models (1) and (5). The multi-interval ERTS smoother consists of forward filtering and backward smoothing, consequently, we will design the UFIR filter and the R-T-S smoother based on the models (1) and (5), secondly, the online multi-interval R-T-S smoothing scheme will be investigated.

3.1 EFIR Filter Algorithm

In the multi-interval ERTS smoother, the EFIR filter is employed as the forward filtering algorithm of the EFIR R-T-S smoother. The EFIR filter algorithm for the models (1) and (5) is sketched in Algorithm 1. We can infer that the performance of the EFIR filter depends on the size ME of the X^l and the filtering window size NE. From the models (1) and (5), we can get that the ME = 4 + 8 = 12, and thus, the performance of the EFIR filter for the models (1) and (5) depends on the NE, which should be set optimally. Moreover, we employ the extend Kalman filter (EKF) (lines 4–8) as the data fusion filter when the EFIR filter does not work in dead zone.

3.2 R-T-S Smoothing Algorithm

When the forward EFIR filter works, the X^l and P^l,l∈[1,L] should be stored. Here, the L means the last time index of the forward filtering. The R-T-S smoothing algorithm can smooth the outputs of the EFIR filter from the time index L to the time index Lb in reverse order, here, the ‘b’ means the last time index for the smoothing algorithm. Employing the X^l and P^l,l∈[Lb,L], the pseudo code of the R-T-S smoothing algorithm based on the models (1) and (5) can be listed in Algorithm 2.

In the R-T-S smoothing algorithm, we set the X^lS=X^l,l∈[Lb,L] and P^lS=P^l,l∈[Lb,L], the Pl−1|lS can be computed as the Eq. (6).

Pl−1|lS=APlSAT+Q,(6)

where superscript ‘S’ means the smoother. The smoothing gain Kl can be computed as the Eq. (7).

Kl−1S=Pl−1SAT(Pl−1|lS)−1,(7)

Then, Eqs. (8) and (9) calculate X^lS (smoothed state estimation) and P^lS (smoothed error cov.).

X^l−1S=X^l−1S+Kl−1S(X^lS−AX^l−1S),(8)

P^l−1S=Pl−1S+Kl−1S(P^lS−Pl−1|lS)(Kl−1S)T,(9)

3.3 Multi-Interval Smoothing Scheme

From the Algorithm 2, it can be seen easily that the R-T-S smoothing using the X^l and P^l,l∈[1,L] is an off-line mode. In this work, to improve accuracy of filtering and the real-time performance, a multi-interval smoothing scheme is proposed for the east position, north position, and the RN's position respectively. The structure of the multi-interval R-T-S smoothing algorithm is shown in Fig. 2. Based on the models (1) and (5), the east position xl, north position yl, and the RN's position (xi, yi), the smoothing schemes are listed in the follows:

• To the east and the north position (xl, yl), we firstly set one threshold door for the position change rate in the east direction (Δ xl = xl − xl − 1) and north (Δ yl = yl − yl − 1) direction.

• When the Δ xl ≤ door, the east position of the robot is considered as the stance, and we set Lx = l. If it is the first time that ΔxlΔxl−1≥door change to Δxl ≤ door, we set Lbx = l. The R-T-S smoothing algorithm will be work for smoothing the EFIR filter outputs X^l and P^l from the time index Lx to the time index Lbx. Since the robot's east position is stance, we can compute the xl using the Eq. (10). When the Δx > door, the EFIR filter's output xl is selected as the output without smoothing.

xl=mean(X^lxS(1,1),…,X^lS(1,1)),(10)

Figure 2: Multi-interval R-T-S smoothing algorithm structure

•   When the Δ yl ≤ door, similar to the previous step, the robot's north position is considered as the stance, we set Ly = l. If it is the first time that Δy l − 1 > door change to Δ yl ≤ door, we set Lby = l. The R-T-S smoothing algorithm will be work for smoothing the EFIR filter outputs X^l and P^l from the time index Ly to the time index Lby. Finally, the yl can be computed by the Eq. (11). When the Δy > door, the yl is output without smoothing.

yl=mean(X^lyS(2,1),…,X^lS(2,1)),(11)

•   To the UWB RN's position (xi,yi),i∈[1,4], since the UWB RNs are always stance, thus, we can smooth the EFIR's outputs using the R-T-S smoothing method from the time index l to time index 1 directly, Then, the (xi,yi),i∈[1,4] can be computed by the Eqs. (12) and (13).

xi=mean(X^1S(j,1),…,X^lS(j,1))i=1,2,3,4,j=5,7,9,11,(12)

yi=mean(X^1S(j,1),…,X^lS(j,1))i=1,2,3,4,j=6,8,10,12,(13)

The multi-interval EFIR R-T-S smoothing for the Eqs. (1) and (5) can be listed in Algorithm 3.

4  Experimental Verification

The following part describes experiments for performance verification of the method using real-world data. First, the experiment configurations are introduced. Second, the results are investigated and performance is analyzed.

4.1 The Setting of the Test

In this subsection, the experiment configurations are introduced. The real test is done inside the 7th building of University of Jinan, which can be seen in Fig. 3. This work employs the UWB localization system, mobile robot, and the computer to test. The UWB localization system employs the DW−1000 based localization system, which includes one UWB BN (fixed on the mobile robot) and four UWB RNs (fixed on the selected positions so that the positions of RNs are stance), The BN can measure the ranges diU,i∈[1,4] from the UWB RNs to the UWB BN fixed on the mobile robot. Then, the diU,i∈[1,4] can be sent to the computer via RS232. The mobile robot with the UWB BN is shown in Fig. 4. We set Δl = 0.5 s in this work. It should be emphasized that the positioning accuracy of the DW−1000 based localization system used in this work under weak occlusion is about 0.20 m, thus, we set the door = 0.20/5 = 0.04 in Algorithm 3, which means that when the estimated displacement of the mobile robot's position in a period is less than 0.04 m, we consider that the robot is stance in this direction. The X0, P0, Q, and R can be set as the follows:

P0=I12×12,Q=I12×12,R=I4×4,(14)

X0=[0,0,0,0,−0.85,7.21+0.15,4.8+0.1,7.21,−0.85+0.15,−2.04−0.05,4.8−0.15,−2.04]T,(15)

where ‘+’ and ‘-’ mean the errors of the UWB RN's position. For example, ‘7.21 + 0.15’ means the the errors of the UWB RN's position is 0.15 m.

Figure 3: The test environment in the 7th building

Figure 4: The mobile robot with the UWB BN

From the models (1) and (5) (through EFIR filter), we can get that the ME = 12. And the optimal NE can be found by the follows:

NoptE=arg⁡minNE⁡{tr(Pl(NE))},(16)

where

Pl=E{(Xl−X^l)(Xl−X^l)T},(17)

The effect of NEN on the localization error using a set of independent data with 200 sampling points is sketched in Fig. 5. From the figure, one can see that the NoptE=13, thus we set NE = 13 in this work.

4.2 The Performance of the Multi-Interval ERTS Smoother for the Trajectory Estimation

In this section, the performances of the UWB, EFIR, and the multi-interval ERTS smoother will be investigated. Fig. 6 shows the trajectories measured by the UWB, EFIR, and the multi-interval ERTS smoother. Fig. 7 shows errors in the east and north position and as we can see, both the EFIR filter and the multi-interval ERTS smoother can reduce the localization errors when being compared to the UWB. Also, the proposed multi-interval ERTS smoother's path is much closer to the ground truth path. Table 1 shows the root mean square errors (RMSEs) given by UWB, EFIR, and multi-interval ERTS smoother. We can see that the proposed multi-interval ERTS smoother has the lowest position error. East- and north-direction positioning errors of the multi-interval ERTS smoother are 0.1175 and 0.0932 m, which improved by 25.36% and 36.92% respectively compared with the EFIR filter. It should be pointed out that the EFIR filter is unsuitable for the estimation of the north position due to the uncertain beacons. Fig. 8 shows the displacements in directions east (Δ xl) and north (Δ yl). Fig. 8 also shows the east- and north-directioned judgment for stance.

Figure 5: The effect of NEN on the localization error

Figure 6: The trajectories measured by the UWB, EFIR, multi-interval ERTS smoother, and the planned path

Figure 7: The localization error. Directions: (a) East (b) North

Figure 8: The localization error. Directions: (a) East (b) North

4.3 The Performance of the Multi-Interval ERTS Smoother for the Estimation of the UWB RNs’ Position

In this section, the performances of the EFIR and the multi-interval ERTS smoother for the estimation of the UWB RNs’ position will be investigated. Fig. 9 shows the estimation of the UWB RNs’ position using the EFIR and the multi-interval ERTS smoother. The RMSEs produced by the EFIR and the multi-interval ERTS smoother for UWB RN #1, #2, #3, and #4 are sketched in the Tables 2–5. From the figure and the tables, we can see obviously that the multi-interval ERTS smoother is effectively to estimate the UWB RN's position. Compared with the EFIR filter, the proposed multi-interval ERTS smoother improved by the 83.51%, 81.61%, 57.44%, 37.73%, 23.92%, 75.36%, 53.75%, and 62.37%.

Figure 9: The estimation of the UWB RN’s position using the FIR and multi-interval ERTS smoother

5  Conclusion

Range-only UWB SLAM for robot localization employing multi-interval EFIR R-T-S smoother has been proposed in this work. The contribution of this work resides in the following:

•   An indoor robot's localization scheme is designed for fusing the range-only UWB measurement.

•   A new EFIR R-T-S smoothing algorithm using the EFIR filter and the R-T-S smoothing for forward filtering and the backward smoothing method is designed.

•   A new smoothing scheme is proposed for the estimation of the east position, north position, and the UWB RN's position, respectively.

•   An experiment using real-world data is conducted and verifies that the proposed multi-interval EFIR R-T-S smoothing algorithm improves the performance.

Funding Statement: The authors received no specific funding for this study.

Conflicts of Interest: The authors declare that they have no conflicts of interest to report regarding the present study.

This work is licensed under a Creative Commons Attribution 4.0 International License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.