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

Yanli Gao,Wanfeng Ma,Jing Cao,Jianling Qu and Yuan Xu,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

ABSTRACT For improving the localization accuracy,a multi-interval extended finite impulse response(EFIR)-based Rauch-Tung-Striebel(R-T-S)smoother is proposed for the range-only ultra wide band(UWB)simultaneous localization and mapping(SLAM)for robot localization.In this mode,the EFIR R-T-S(ERTS)smoother employs EFIR filter as the forward filter and the R-T-S smoothing method to smooth the EFIR filter’s output.When the east or the north position is considered as stance,the ERTS is used to smooth the position directly.Moreover,the estimation of the UWB Reference Nodes’(RNs’)position is smoothed by the R-T-S smooth method in parallel.The test illustrates that the proposed multi-interval ERTS smoothing for range-only UWB SLAM is able to provide accurate estimation.Compared with the EFIR filter,the proposed method improves the localization accuracy by about 25.35%and 40.66%in east and north directions,respectively.

KEYWORDS Robot localization; ultra wide band; Rauch-Tung-Striebel smoother; extended FIR filter

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/GPSintegration 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 dUi,i∈[1,4] fromithRN to BN.Then,the ranges dUi,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

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).

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

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

where the itemsxl,yl,Vxl,Vylis 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 indexlfor time,the AR4×4in Eq.(1)is listed in Eq.(4).

where theΔlis the sampling interval.

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

where the,i∈[1,4] is range from theithUWB RN to the mobile robot at the time indexl,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 sizeMEof theand the filtering window sizeNE.From the models (1)and (5),we can get that theME=4+8=12,and thus,the performance of the EFIR filter for the models (1)and (5)depends on theNE,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.

Algorithm 1:EFIR filter algorithm for the models (1)and (5)Data:Yl, ME, NE,Q,R 1 Return:ˆXl,ˆPl 2 for l= 1:L do 3If l< NE then 4ˆXl|l−1=A ˆXl−1;5ˆPl|l−1=A ˆPl−1AT+Q;6Kl=Pl|l−1HTl (HlPl|l−1HlT+R)−1;7ˆXl= ˆXl|l−1+Kl(Yl −h(ˆXl|l−1));8ˆPl=(I −KlHl)ˆPl|l−1;9else 10m =l−NE + 1, t=m+ M E −1;11˜Xt= ˆXt;12˜Pt= ˆPt;13Gt= I;14for p=m+ ME:l do 15˜Xp|p−1=A ˜Xp−1 16˜Pp|p−1=A ˜Pp−1AT+Q;17Gp=[HpTHp+(AGp−1AT)−1]−1;18Kp=GpHpT;19˜Xp= ˜Xp|p−1+Kp(Yp −h(˜Xp|p−1));20˜Pp=(I −KpHp)˜Pp(I −KpHp)T+KpRKpT;21end for 22ˆXl= ˜Xp;23ˆPl= ˜Pp;24 end if 25 ˆXl= ˆXl;26 ˆPl= ˆPl;27 end for 28 return:ˆXl,ˆPl 29 † ME:the size of ˆXl 30 † NE:the filtering window size 31 †L:the total time index number 32 †Hl= ∂h(Xl)∂Xl

3.2 R-T-S Smoothing Algorithm

When the forward EFIR filter works,theand,l∈[1,L] should be stored.Here,theLmeans 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 indexLto the time indexLbin reverse order,here,the‘b’means the last time index for the smoothing algorithm.Employing theand,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 thethecan be computed as the Eq.(6).

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

Then,Eqs.(8)and (9)calculate(smoothed state estimation)and(smoothed error cov.).

Algorithm 2:Eqs.(1)and (5)R-T-S smoothing Data:ˆXS l ,l ∈[Lb,L],ˆPS l ,l ∈[Lb,L],Q,R 1 Return:ˆXS l ,ˆPS l 2 for l=L:Lb do 3ˆPS l−1|l=A ˆPSl AT+Q;4KSl−1= ˆPS 5ˆXS l−1AT(ˆPS l−1|l)−1;l−1= ˆXS 6ˆPS l−1+KSl−1(ˆXS l −A ˆXl−1);l−1= ˆPS 7 end for 8 return:ˆXS l−1+KSl−1(ˆPS l −ˆPS l−1|l)(KSl−1)T;l ,ˆPS l 9 † ‘S’means the smoother

3.3 Multi-Interval Smoothing Scheme

From the Algorithm 2,it can be seen easily that the R-T-S smoothing using theand,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 positionxl,north positionyl,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 thresholddoorfor 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 setLx=l.If it is the first time thatΔxl−1≥doorchange toΔxl≤door,we setLbx=l.The R-T-S smoothing algorithm will be work for smoothing the EFIR filter outputsandfrom the time indexLxto the time indexLbx.Since the robot’s east position is stance,we can compute thexlusing the Eq.(10).When theΔx>door,the EFIR filter’s outputxlis selected as the output without smoothing.

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 setLy=l.If it is the first time thatΔyl−1> door change toΔyl≤door,we setLby=l.The R-T-S smoothing algorithm will be work for smoothing the EFIR filter outputsandfrom the time indexLyto the time indexLby.Finally,theylcan be computed by the Eq.(11).When theΔy>door,theylis output without smoothing.

• 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 indexlto time index 1 directly,Then,the(xi,yi),i∈[1,4] can be computed by the Eqs.(12)and (13).

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

Algorithm 3:The multi-interval EFIR R-T-S smoothing algorithm for the models (1)and (5)Data:Yl, ME, NE,Q,R, door Result:ˆXl 1 begin 2for l= 1:L do 3Compute the ˆXl and ˆPl using Algorithm 1;4Smooth the EFIR’s outputs using Algorithm 2;5Compute the (xi,yi),i ∈[1,4] by the Eqs.(12)and (13);6if l>1 then 7 if Δxl ≤door then 8 Lx =l;9 if Δxl−1 > door then 10Lbx =l;11end if 12Smooth the ˆXl and ˆPl from the time index Lx to the time index Lbx using Algorithm 2;13Compute the xl by the Eq.(10);14end if 15if Δyl ≤door then 16Ly =l;17if Δyl−1 > door then 18Lby= l;19end if 20Smooth the ˆXl and ˆPl from the time index Ly to the time index Lby using Algorithm 2;21Compute the yl by the Eq.(11);22end if 23end if 24 end for 25 end

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 rangesdUi,i∈[1,4] from the UWB RNs to the UWB BN fixed on the mobile robot.Then,thedUi,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 thedoor=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:

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 theME=12.And the optimalNEcan be found by the follows:

where

The effect ofNEN 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=13,thus we setNE=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 multiinterval 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 multiinterval 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.

Table 1:RMSEs comparison of different methods

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

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%.

Table 2:EFIR and the multi-interval ERTS smoother RMSEs of the 1st UWB RN

Table 3:EFIR and the multi-interval ERTS smoother RMSEs of the 2nd UWB RN

Table 4:EFIR and the multi-interval ERTS smoother RMSEs of the 3rd UWB RN

Table 5:RMSEs produced by the EFIR and the multi-interval ERTS smoother for UWB RN #4

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

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 multiinterval 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.