Feature extension and matching for mobile robot globallocalization

Peng Wang,Qibin Zhang,and ZonghaiChen

Departmentof Automation,University of Science and Technology of China,Hefei230027,China

Feature extension and matching for mobile robot globallocalization

Peng Wang,Qibin Zhang,and ZonghaiChen*

Departmentof Automation,University of Science and Technology of China,Hefei230027,China

This paper introduces an indoor global localization method by extending and matching features.In the proposed method,the environment is partitioned into convex subdivisions. Localextended maps of the subdivisions are then built by extending features to constitute the local extended map set.While the robotis moving in the environment,the localextended map of the currentlocalenvironmentis established and then matched with the localextended map set.Therefore,globallocalization in an indoor environment can be achieved by integrating the position and orientation matching rates.Both theoreticalanalysis and comparison experimentalresult are provided to verify the effectiveness of the proposed method for globallocalization.

feature extension,global localization,feature matching,mobile robot.

1.Introduction

As one ofthe prerequisites formobile robots to achieve autonomous navigation,localization tries to estimate position of a robotin an environment,with the assumption thatthe robotis provided with a map of the environment[1,2].Localization mainly focuses on two issues:localpose tracking and global localization.Local pose tracking aims at precise localization.Optimal states of the robot and features are estimated using filtering techniques to eliminate system and measurement noises[3–5].Global localization,on the other hand,usually exploits the topological modeland locates the robotin a certain localenvironment corresponding to a topological node by feature matching techniques[6].

Global localization methods can be classified into two categories:probabilistic filter-based approach and scan matching-based approach.The probabilistic filter-based approaches estimate a robot’s position by recursively update conditionaldistributions of the robotstates.Typical examples like multi-hypothesis Kalman filters[7]and gridbased filters[8]are both widely used.The scan matchingbased approaches,on the otherhand,have also attracted attention as they are more intuitive,reliable and accurate[6]. Generally,there are two classes of scan matching-based approaches:global scan matching method and sequential scan matching method[6].The global scan matching method matches geometric features,which should be detected first.Zhang et al.[9]represent an environment using closed line segment,and the globallocalization is performed by finding relevantline segments in the map.Other methods based on global scan matching can be found in [10–12].The sequentialscan matching method,however, matches successive scans with an alignment assumption between them.The most popular sequential scan matching method is the iterative closestpoint(ICP)method[13]. Its application in globallocalization can be found in[6].

However,in the above mentioned globalscan matching methods,only line or corner estimates are used.In case there are estimation errors,such methods may failto locate the robot.Sequential scan matching method is comparatively robust because it uses sensor scans directly.However,when a bad alignment happens,the sequential scan matching may result in a local minimum.To reduce such risks,we propose a novel localization method in this paper.For an indoorenvironment,we firstdivide it into convex subdivisions according to[14,15].Then,we compute error ellipses of features from measurements[16]to construct maximum error circles.Therefore,local extended map(LEM)set can be built up by extend local environments using maximum error circles and their connections offline.While the robot is moving in the environment, we use the ring projection transformation(RPT)method and the two dimensional scan matching(2DSM)method to carry out position matching,and the eigenvalue ratio (EVR)method to complete orientation matching.By integrating the position and orientation matching rates,wedetermine where the robotis located.

The remainderofthis paperis organized as follows.Section 2 describes how features are extended to build LEMs and LEM set.Section 3 introduces the RPT,2DSM,and orientation matching algorithms,the probability integration process is also given.Section 4 provides the experimental results and analysis,and Section 5 concludes this paper.

2.Feature extension and LEM set

According to[15],an indoor environment can be partitioned into a set of convex subdivisions E1,E2,...,Embased on corner features,as shown in Fig.1.In this paper,we extend geometric features like corners and lines by using the error ellipse.

Fig.1 Partition results of an indoor environment

Suppose that[xy]are measurements of a corner O,in which x=(x1,...,xn)Tand y=(y1,...,yn)T.Then, we compute covariance matrix M of x and y using(1).

Suppose that a and b(a≥b)are eigenvalues of M,v1and v2are the corresponding eigenvectors.Then error ellipse of the n measurements is given in(2)with geometric

where the majoraxis and the minoraxis ofthe errorellipse are along v1and v2,respectively.

Based on what is mentioned above,we have the following definitions.

Definition 1The maximum errorcircle is a circle centered atand with a radius a/2.

Definition 2The LEMof a subdivision is builtby representing the subdivision using maximum error circles and their connections.

In this paper,two classes of LEMs are built:the current LEM(CLEM)and the targetLEM(TLEM).The CLEMis built from current observations,and the TLEM is defined as an LEMin the LEMsetthatindicates possibly the same subdivision in the environment.

Definition 3The background template(BT)is a square which contains D×D small square grids,where D is a naturalnumber.

For convenience,the BT is set with the same scale of the CLEM(or TLEM),and D is set to be an even number. Area of the grids is determined by applications.

Fig.2 shows the LEM and BT of E17in Fig.1.In this paper,we first build LEMs of all subdivisions in Fig.1 and constitute an LEM set offline.Then,while the robot is moving in the environment,CLEM of the current local environmentis builtand matched with the LEM setto determine where the robotis located.

Fig.2 LEMof E17and the corresponding BT

3.LEMmatching

3.1 RPT matching

We denote centerpoints of the CLEM,the TLEM,and the BT as Pc(xc,yc),Pt(xt,yt),and Pb(xb,yb),respectively. Pi(x?i,yi)is the coincidentpoint.Then the coordinate systemshown in Fig.3(a)is established with the origin point Pi.A ring Rrcentered at Piis constructed by two circles,with radiuses r and r+s.Fig.3(b)shows the RPT matching result of E17.The RPT matching algorithm is shown as follows:

Algorithm 1RPT algorithm

Input:CLEM,LEMset

Output:match rate

Compute Pt,Pi,Pband Pc,build

Construct Rr,compute v(r)=Ur/Nr;

Construct vcand vt;

Compute the match rate prpt.

In Algorithm 1,Nris the numberof BT elements(small square grids)occupied by Rr.The elements of Rroccupied by the LEM is Ur.v(r)=Ur/Nris the duty ratio(DR)of the LEM at the radius r.DR vectors vc= (vc(0),...,vc(D/2))Tand vt=(vt(0),...,vt(D/2))Tof the CLEMand TLEMare computed with radius stretchingfrom zero to D/2.The normalized correlation coefficient (NCC)of vcand vtcan be calculated by(3)

indicates the match rate of the CLEMand the TLEM.

Fig.3 RPT matching method

3.2 2DSM

The reliability of the RPT matching is sometimes reduced due to information oversimplification[17,18].Thus,we propose the 2DSMmethod to improve the position matching rate.

Normally,if the CLEM and the TLEM represent the same local environment,by rotating the TLEM with an angle,we will find out that the TLEM will overlap with the CLEM.Fig.4(a)shows an example of the 2DSM and Fig.4(b)is the matching result.Suppose that grids occupied by the LEM in the j th row and column of the BT are Nrjand Ncj.We then define the row DR and column DR as wrj=Nrj/D and wcj=Ncj/D,respectively.

Fig.4 2DSMmethod

are row and column DR vectors ofare NCC vectors.We then propose Algorithm 2 to compute the 2DSMrate ofthe TLEMand the CLEMas follows:

Algorithm 22DSMalgorithm

Input:CLEM,TLEM

Output:ρr,ρc

for i=1:K

end for

Normally,after rotate the TLEM with an angle,it will overlap the CLEM,i.e.,the maximum elements ofρrand ρcare of the same serial number.However,due to system perturbations and measurementerrors,angles corresponding to the maximum row and column NCC elements are different.We solve the problem as follows.corresponding to the maximum elements of the row and column NCC vectors separately.

(ii)GivenΔθrandΔθc,we first calculateThen,we compute the average row and column NCCs correspondingLikewise,the average row and column NCCs corre-are computed and

We compute row and column matching rates of the CLEM and TLEM separately through/2.Then the finalmatching rate of the 2DSMis computed by p2d=(px+py)/2.

3.3 Orientation matching

Four typical local environments in indoor environments and the corresponding segmentations are shown in Fig.5. According to[19],we can obtain orientation information of the robot locating in such environments by using the EVR method.Therefore,we use the orientation matching as a supplementto improve the position matching rate.

Fig.5 Typicallocalenvironments and segmentations

Suppose that Scis a setof measurements of the current

localenvirontment,andΣHis the coordinate system.are segmentations of Sc,whereandindicate data above and below x axis,cate data atthe leftand rightof y axis.

We define a slide window with size N and slide directioncan be computed.We first select N elements along→νwith xc=(x1,x2,...,xN)Tand yc=(y1,y2,...,yN)T.Then,we compute the covariance matrix Mcof xcand ycusing(1).Suppose thatλmaxandλminare eigenvalues of Mc,we then define EVR of the currentwindow asλmin/λmax.By sliding the window along→νstep by step,we construct EVR vectorby calculating all EVRs.Similarly,we construct EVR vec-

tors in other three directions and denote them as

Select arbitrarily a vector of the current environmentwith constantstep.Samples along x axis form a vector and we denote it asLikewise,we sample vectors of the target local environment along the same axis,and denote the sample vectorsThen,we compute similarities(i=1,2,3,4)using(4).

where the firstpartindicates the orientation matching rate,are sample numbers,αis the adjusting factor.

Through(4),we get the original similarity vector?=(?1,?2,?3,?4).Then?maxis constructed by computing the maximum elements of the four originalsimilarity vectors separately.Suppose the numberofelements greaterthan the set threshold?thin?maxis Nmax,then the final orientation matching rate is pdir=Nmax/4.

3.4 Globallocalization

Fig.6 shows the global localization algorithm flow chart. Details are given below.

Fig.6 Localization algorithm flow chart

3.4.1 Currentmatching probability

(i)Build the CLEM ELbased on observations of the currentlocalenvironment;

(ii)Match ELwith E={E1,E2,E3,...,Em}. By fusing the RPT and 2DSM matching rates,we get Pmat=(pmat(E1),...,pmat(Em))with pmat(Ej)= prpt(Ej)·p2d(Ej).Then we normalize Pmatand denote the result as P=(p(E1),...,p(Em))withThe maximum normalized probability in P is denoted as pmax(Ej)whichmeans that Ejhas the highest probability pmax(Ej)= p(zt|Ej,u1∶t)thatthe robot is located in it(1?j?m), z is measurementand u is action.

3.4.2 State transition probability

Suppose probabilities that the robot is located in E are Pt?1=(p(E1|u1∶t,z1∶t?1),...,p(Em|u1∶t?1,z1∶t?1)at time t?1.The robotmoves from Ehto Ejwith probability p(Ej|Eh,u1∶t,z1∶t?1)after the action ut|t?1,1?h?m. Then,at time t,the probability that the robotis located in Ejcan be calculated by(5).

We denote the expectation of the odometer after the action ut∶t?1as?d with varianceσ2.The distances from Eh(h=1,2,...,m)to Ejare denoted as Do=are the absolute differences between di(i=1,...,m)andmax(Du),dmin=min(Du).Then,the state transition probability p(Ej|Eh,u1∶t,z1∶t?1)can be calculated as follows.

(i)Divide[dmin,dmax]into 2k+1 subintervals I=1)σ,...,dmid+(2k?1)σ,dmax}as boundary points.

(ii)Count numbers of the elements in Duthat be-which are denoted as N=then the probability thatthe robotmoves from Ehto Ejafter the action ut|t?1is computed by(6).

3.4.3 Probability integration

(i)By integrating the current matching probability and the state transition probability,we compute the probability thatthe robotis located in Ejby(7),

whereκis the normalization factor.

(ii)We then integrate p(Ej|u1∶t,z1∶t)and pdirusing(8).

4.Experiment results and analysis

The experiments are conducted in an environmentshown in Fig.1.The Pioneer 3-DX robot,which is equipped with an odometer,a gyroscope and 16 ultrasonic sensors with maximum measurement range of 5 m,is employed to carry out the experiments.The sampling rate is 24 Hz. For comparison,the ICP matching method is also applied for globallocalization.We willfirstdescribe results of the proposed method.Then,comparisons with the ICP method are given.

Fig.7 shows real sonar data of E17and the maximum errorcircle of one corner.Fig.8 shows the position matching rates and the final localization results.Fig.8(a)and Fig.8(b)show the row and column matching rates of the 2DSM.It can be seen that the row and column matching rates of E10are greater compared to the rates of other subdivisions.Fig.8(c)shows the RPT matching rates.We can find outthatthe matching rate of E10is the greatest.Both the RPT matching and 2DSMrates are consistentwith the fact that the robot is located in E10.Fig.8(d)is the integrated probability.We can easily conclude from the results thatthe robotis located in E10.Fig.8(e)shows the results by integrating the orientation matching rates with position matching rates.It can be seen that matching rates of E9and E13are dramatically decreased due to the similar spatial structure.To improve the localization accuracy,more information should be accumulated.Fig.8(f)shows the final localization results by integrating position matching rates and the orientation matching rates.The ICP method is also used in the same environment.Besides the alignment,the performance of the ICP method is also affected by the amount of measurements.As shown in Fig.9,the failure of the ICP method caused by scan insufficiency directly leads to wrong matching results between the current and targetlocalenvironments.Thus,a new corridoris built and global localization thereafter fails.The final localization results of the two methods are shown in Fig.10.We can conclude that when there are not sufficient measurements,the proposed method is more rubust and reliable than the ICP method.

Fig.7 Error circle built from real data

Fig.8 Localization results

Fig.9 Wrong match ofthe ICP method

Fig.10 Comparison ofthe two methods

5.Conclusions

In this paper,a globallocalizaiton method is proposed.In the proposed method,we partition an indoor environment into convex subdivisions.Then,the CLEMand the TLEM are matched by using the RPT and 2DSMmethods.To furtherimprove the matching rate,we propose the orientation matching method.

While the robotis moving in the environment,the position and orientation matching rates are integrated to compute the current matching probability.The state transition probability is also obatained from the motion model.Then, the current matching probability and the state transition probability are integated.Experimentresults show thatthe proposed method provides relatively robustand reliable localization results in indoorenvironments.

[1]G.Lakemeyer,B.Nebel.Exploring artificial intelligence in the new millennium.San Francisco:Elsevier Science,2003.

[2]S.Thrun,D.Fox,W.Burgard,etal.Robust Monte Carlo localization formobile robots.ArtificialIntelligence,2001,128(1): 99–141.

[3]M.Choi,J.Choi,W.K.Chung.Correlation-based scan matching using ultrasonic sensors for EKF localization.Advanced Robotics,2012,26(13):1495–1519.

[4]L.Teslic,I.Skrjanc,G.Klancar.EKF-based localization of a wheeled mobile robot in structured environments.Journal of Intelligent&Robotic Systems,2011,62(2):187–203.

[5]R.Havangi,M.Teshnehlab,M.A.Nekoui,etal.A novelparticle filter based SLAM.International Journal of Humanoid Robotics,2013,10(3):1–23.

[6]S.Park,S.K.Park.Globallocalization for mobile robots using reference scan matching.International JournalofControl, Automation and Systems,2014,12(1):156–168.

[7]K.O.Arras,J.A.Castellanos,R.Siegwart.Feature-based multi-hypothesis localization and tracking for mobile robots using geometric constraints.Proc.of the 19th IEEE International Conference on Robotics and Automation,2002:1371–1377.

[8]J.Reuter.Mobile robotself-localization using PDAB.Proc.of the 17th IEEE International Conference on Robotics and Automation,2000:3512–3518.

[9]L.Zhang,B.K.Ghosh.Line segmentbased map building and localization using 2D laser rangefinder.Proc.ofthe 17th IEEE International Conference on Robotics and Automation,2000: 2538–2543.

[10]J.Weber,L.Franken,K.W.Jorg,etal.Reference scan matching forglobalself-localization.Robotics and Autonomous Systems,2002,40(2):99–110.

[11]M.Tomono.A scan matching method using euclidean invariant signature for global localization and map building.Proc. of the 19th IEEE International Conference on Robotics and Automation,2004:866–871.

[12]G.Grisetti,L.Iocchi,D.Nardi.Globalhough localization for mobile robots in polygonal environments.Proc.of the 19th IEEE International Conference on Robotics and Automation, 2002:353–358.

[13]Z.Y.Zhang.Iterative point matching for registration of freeform curves and surfaces.International Journal of Computer Vision,1994,13(2):119–152.

[14]S.Han,S.J.Li,Z.H.Chen.New representation method for uncertain knowledge-grey probability measure set.Systems Engineering and Electronics,2013,35(5):1018–1022.(in Chinese).

[15]S.J.Li,P.Wang,Z.H.Chen.An environment modelfor mobile robot:grey qualitative map.Jiqiren/Robot,2012,34(4): 476–484.(in Chinese).

[16]R.S.M.S.P.Cheeseman,R.Smith,M.Self.A stochastic map for uncertain spatialrelationships.Proc.of the 4th International Symposium on Robotics Research,1987.

[17]J.Choi,M.Choi,S.Y.Nam,et al.Autonomous topologicalmodeling of a home environment and topological localization using a sonar grid map.Autonomous Robots,2011,30(4): 351–368.

[18]Y.H.Choi,T.K.Lee,S.Y.Oh.A line feature based SLAM with low grade range sensors using geometric constraints and active exploration formobile robot.Autonomous Robots,2008, 24(1):13–27.

[19]K.Lee,N.Cho,W.K.Chung,et al.Topological navigation of mobile robotin corridorenvironmentusing sonarsensor.Proc. of the 19th IEEE/RSJ International Conference on Intelligent Robots and Systems,2006:2760–2765.

Biographies

Peng Wangwas born in 1988.He is now a Ph.D. student atthe Laboratory of Simulation and Intelligent Control in the Department of Automation, University of Science and Technology(USTC). Hisresearch interests include system modeling and simulation,mobile robot localization and knowledge representation.

E-mail:pwang@mail.ustc.edu.cn

Qibin Zhangwas born in 1991.He is now an M.S. student at the Laboratory of Simulation and Intelligent Control in the Department of Automation, University of Science and Technology(USTC).His research interests include mobile robot localization and landmark extraction,knowledge representation and intervalanalysis.

E-mail:zqb101@mail.ustc.edu.cn

Zonghai Chenwas born in 1963.He is currently a professor atthe Laboratory of Simulation and Intelligent Control in the Department of Automation, University of Science and Technology(USTC).His research interests include simulation and optimization controlof complex system,theory and technology of intelligent system,and quantum control theory.

E-mail:chenzh@ustc.edu.cn

10.1109/JSEE.2015.00091

Manuscript received March 31,2014.

*Corresponding author.

This work was supported by the National Natural Science Foundation of China(61375079).