Laser Radar based Vehicle Localization in GPS Signal Blocked Areas

Transcription

1 International Journal of Computational Intelligence Systems, Vol. 4, No. 6 (December, 20), Laser Radar based Veicle Localization in GPS Signal Bloced Areas Ming Yang Department of Automation, Sangai Jiao Tong University Key Laboratory of System Control and Information Processing, Ministry of Education of Cina Sangai, , Cina Cunxiang Wang *, Hui Fang Researc Institute of Robotics, Sangai Jiao Tong University Sangai, , Cina Bing Wang Department of Automation, Sangai Jiao Tong University Key Laboratory of System Control and Information Processing, Ministry of Education of Cina Sangai, , Cina Abstract Reliable veicle localization is a basic requirement in many applications in transportation field. GPS-based localization is quite popular nowadays. However, in urban environments applications, signal of GPS is often bloced by surrounding objects lie ig-rise buildings, tunnels, overead roads, etc, maing localization information unavailable. Tis paper proposed a laser radar based map matcing approac to address tis problem, especially wen GPS signal bloced area is large. Te proposed approac includes mapping and localization. In te mapping, after map initialization sensor data constraints are linearized to formulate an optimal linear estimation based map optimization framewor, wic can improve map accuracy effectively. In te localization, veicle pose is estimated by matcing te current laser scan wit te best submap and by a UKF (Unscented Kalman Filter) based fusion strategy. Results from bot syntetic and real experiments sow good performance of te proposed approac. Keywords: GPS Signal Bloced, Veicle Localization, Mapping, Map Optimization. Introduction Veicle localization is a fundamental tas in many applications. In-car navigation systems use its localization module to compute veicle's current position and sow it on a digital map to tell drivers ow to get from one location to anoter efficiently. Wit a localization system, veicle can communicate its position to a monitoring center, wic will elp operators to direct veicle fleets as efficiently as possible since tey now te traffic flow on different roads. Furtermore, veicle localization is also quite important in intelligent transportation systems, suc as Advanced Driver Assistance Systems and driverless veicles. Tey require reliable and accurate localization to mae intelligent decisions or realize automatic navigation. Global Positioning System (GPS) as been widely used in veicle localization systems 2,3. However, a big problem wit GPS-based localization is tat GPS signal is often bloced by surrounding objects, especially in urban environments 4. For example, ig-rise buildings, tunnels, overead roads or even tall trees can bloc GPS signals seriously, maing localization information unavailable. Tis is a quite critical problem, especially * Corresponding Autor: wangcx@sjtu.edu.cn Tis wor was financially supported by te National Natural Science Foundation of Cina (67478) and te Sangai Science and Tecnology Commission troug Expo Science and Tecnology Program (0dz05800)

2 Ming Yang, et al in applications requiring reliable and continuous veicle localization. Many efforts ave been made to tacle tis problem. Additional devices suc as altimeters and precise clocs Ref. ave been used to provide additional information. Ref.5 suggested a constrained metod by modeling te traveling pat as pieces of lines. In tis metod, te minimum number of satellites required is reduced to two. In Ref.6, autors used te constraints tat te altitude of te veicle and veicle speed are approximately constant. Ten tey designed a localization algoritm requiring number of satellites fewer tan 4. A more popular approac is te integration of GPS wit Inertial Navigation Systems (INS) or dead reconing sensors suc as encoders to provide continuous localization information 2. Altoug many approaces ave been proposed, most of tem assumed tat te duration of te blocage of GPS signals is sort (te GPS signal bloced area is small) or tere is still several satellites can be viewed. How to localize a veicle in a large bloced area witout visible satellites? Tis is our motivation to carry out te wor in tis paper. In large GPS signal bloced areas, map matcing can be used to address te localization problem. It sould be noted tat te map matcing ere is quite different from te one often used in veicle navigation systems, in wic map matcing is realized by matcing te current veicle location and istory trajectory wit te road networ of a digital map 7. In our case, map matcing is carried out by matcing sensor measurements wit an environmental map 8, wic is often used in robotic field. In map matcing, one difficulty is ow to create a precise map. Simultaneous Localization and Mapping (SLAM) 9 is an attractive tecnique for veicle navigation in an unnown environment. However it may not be suitable for real-time accurate localization if it s applied in outdoor unstructured environments. Additionally, SLAM is not necessary for veicle localization in urban environments (actually in GPS signal bloced areas) since te environments are nown to us. Terefore, we can just first create a map and ten use te map to localize a veicle, wic can ensure reliability and efficiency. Te proposed approac for veicle localization in GPS signal bloced areas includes two steps: mapping and localization. Mapping is done offline and localization is real-time. In te mapping step, a veicle is manually guided to explore environment (bloced area), recording sensor data from laser radar and odometry wit time stamps. Additionally, GPS data in entrance and exit of bloced area is also recorded for mapping. Mapping algoritm utilizes tese sensor data and an optimization strategy to create a precise map. Optimization is acieved based on linearizing constraints from sensor data. In te next step, te created map can be sared wit oter veicles to localize temselves by map matcing. Map matcing applies a robust version of ICP (Iterative Closest Point) algoritm registering te current laser frame wit te best submap. A UKF (Unscented Kalman Filter) based fusion strategy is also designed to improve reliability by fusing map matcing result wit dead reconing. Te rest paper is organized as follows: section 2 firstly discusses te pairwise range scans registration problem, wic is a fundamental tecnique in our mapping and localization approac; ten section 3 describes ow te recorded data is processed to create an accurate map. In section 4, a localization algoritm including map matcing and fusion is presented. Next, section 5 sows experimental results wit bot syntetic and real data; finally, section 6 ends tis paper wit some conclusions. 2. Pairwise Range Scans Registration Laser radar is used to collect environmental information in GPS signal bloced area. Te laser radar was fixed on te top of te veicle to reduce te effect of moving object. Te output of laser radar is called range scan, wic is a list of points corresponding to te intersection of a laser beam wit objects in te environment. Te laser beam rotates in a orizontal plane. Tus a range scan describes a 2D slice of te environment. Bot mapping and localization in te proposed approac are based on pairwise range scans registration. Tis tecnique is usually applied to compute translation and rotation between two frames to estimate ego motion of a veicle. ICP is a useful algoritm for addressing te pairwise registration problem 0. It selects te closest point as corresponding point and ten estimates registration parameters by minimizing te value of a given error function. Tese two steps are iteratively implemented to obtain an accurate result. Publised by Atlantis Press Copyrigt: te autors 0

3 Laser radar based Veicle Localization Establising correct corresponding points is crucial in ICP. Here, we apply following tree considerations for determining correspondences: ) Corresponding points as sortest Euclidean distance in world reference frame. Furtermore tis Euclidean distance as to be witin te uncertainty of odometry data (odometry data is used to predict veicle poses). 2) Corresponding points sould ave similar normal direction. 3) Applying M-estimator witin iterative procedure. Given pairs of corresponding points {(P,CP )},,2. Let be (tx, ty, tr) te translation and rotation between two range scans. Te objective of ICP is to estimate (tx, ty, tr) accurately. Te error function of ICP is defined in te form of weigted least-square: EICP ( tx, ty, tr) λ ( Rtr P + T CP) were: T T [, ] CP [ cx, cy ] T [ tx, ty] P x y i cos( tr) sin( tr) Rtr sin( tr) cos( tr) λ is M-estimator coefficient, defined as:, e µ + σ λ σ / e, µ + σ < e µ + 3σ 0, µ + 3σ < e were e is te absolute value of residual error, µ is te mean of e, σ is te standard variance of e. A closed-form solution can be derived as: λ ( )( ) ( )( ) x x cy cy y y cx cx tr arctan λ ( )( ) ( )( ) x x cx cx + y y cy cy tx cx ( x cos( tr) y sin( tr)) ty cy ( x sin( tr) + y cos( tr)) were: λ x λ y x, y, λ λ λ cx λ cy, cy λ λ cx Te procedure of ICP algoritm is illustrated in Fig.. Odometry data is utilized to give initial values of (tx, ty, tr) to start ICP. Proper tresold need to be cosen to T 2 stop te iterative procedure according to te accuracy requirement. Pairwise registration tecnique will be applied bot in mapping and localization. In te mapping, corresponding points determined in te final iterative step are used in map optimization. In te localization, it is used as basic map matcing tecnique to estimate veicle poses. Pairwise registration may fail because of errors in sensor data acquisition and processing. If one of te following cases appens, te result of pairwise registration will be considered trustless: ) Te number of corresponding points establised in final iterative step is smaller tan a tresold (for example, 25). 2) Te number of iterations exceeds te maximal one (for example, 30). 3) Veicle displacement computed by pairwise registration is larger tan a predicted uncertainty (by using odometry data). Fig. Illustration of ICP algoritm 3. Mapping in GPS Signal Bloced Area If te duration of GPS signals blocage is sort, we can integrate dead-reconing wit GPS system to obtain a reliable localization. However, if te blocage duration is long, we ave to apply oter tecnique. Here, we apply map matcing based localization. Tus, te first problem we need to solve is ow to create a precise map. 3.. Coordinate Systems Fig. 2 sows te world reference frame and veicle reference frame denoted by X W O W Y W and X V O V Y V respectively. Te origin of veicle reference frame is in te middle of te rear axle. Veicle pose is denoted by X Publised by Atlantis Press Copyrigt: te autors 02

4 Ming Yang, et al (x, y, r), representing position and orientation of a veicle in te world reference frame. Fig.3 illustrates te relationsip between world coordinates (xw, yw) and veicle coordinates (xv, yv) of a scan point p (assume laser scan points ave been transformed from laser coordinates to veicle reference frame by calibration), wic can be expressed as: xw cos( r) sin( r) xv x yw + sin( r) cos( r) yv y Above equation can be rewritten as: T [ xw, yw] f( xyrxv,,,, yv) f( X, xv, yv) () were f is a non-linear coordinates transformation function. as certain geometric sape, points from natural object are scattered due to its irregular geometric sape and will ave ig standard deviation values bot in x and y directions 2. Fig. 4 sows te clustering and standard deviation analysis results of one laser scan. Only clusters belong to man made objects remain for furter processing. In section 2, we ave mentioned tat normal direction of scan point is used for determining correct correspondences in ICP algoritm. Point s normal direction is computed by fitting to a neigborood of points centered at tat point 3. Furtermore, points wit large fitting error will be removed since tis usually means te normal direction is poorly defined due to ig-curvature points or igly noisy regions. Fig. 5 displays te result, were points enclosed by circles are tose wit large fitting errors. Fig. 2 Coordinates system Fig. 3 Coordinates relationsip 3.2. Scan Data Preprocessing Altoug laser radar as been fixed on te top of te veicle aiming to reduce te effects of moving objects, we still need a special consideration on data preprocessing to extract stable environmental information to create a stable map. Some isolated points in te scan data ave to been removed since tey may be noises. And some unstable natural objects lie tree leaves, wic may reduce reliability in range scans registration, also need to be removed. Te first step of data preprocessing is clustering (also called segmentation). A cluster is a set of scan points close enoug to eac oter tat probably belong to te same object. Te clustering metod applied ere is based on Ref.. Te main idea is to subdivide eac scan into small sets of neigboring points (clusters), considering te proximity between two consecutive points of te scan. After clustering, clusters wit small number of scan points (smaller tan 5) are removed since tey are objects far from current veicle position or noises. Additionally, we analyze te standard deviation (x,y direction) of eac cluster to cec weter it belongs to natural object (lie tree leaves). Te basic idea is tat, unlie man made object wic Fig. 4 Clustering and Standard deviation analysis 3.3. Map Initialization Fig. 5 Normal direction of scan points Te map is divided into lots of submap, wic is called map frame, denoted by M i {X i, S i }. X i represents veicle pose and S i represents laser scan data after preprocessing. Suppose tere are n+ map frames in te map and tus te entire map can be expressed as Map {M, M 2,, M i,, M n }. Laser scan frame rate is important and can be set according to te veicle speed to eep a suitable overlapping size between consecutive laser scans. Tis overlapping redundancy is essential to carry out following map optimization. Te main difficulty in creating a map is ow to get veicle poses. Tis is an ego-motion estimation problem 0. Here, we simply apply odometry based deadreconing to estimate veicle poses. However, large accumulative error will be introduced over long time. Terefore, we need optimization approac to improve map accuracy. Publised by Atlantis Press Copyrigt: te autors 03

5 Laser radar based Veicle Localization After veicle poses as been estimated (may include large errors), we can group eac veicle pose wit corresponding laser scan data Map Optimization In Fig. 6, suppose curve AB is true trajectory of veicle moved in GPS signal bloc area. Because of accumulated errors in dead-reconing, te veicle pose trajectory estimated in map initialization may be lie curve AB, wic will result in large errors in a global map and mae map matcing based localization unsuccessful. So we need to improve veicle pose accuracy to obtain a precise map. Fig. 6 Veicle trajectory error in initial map Some metods ave been presented to improve map accuracy. Zao 4 first incrementally created a map by sequential matcing. Because of error accumulation, tere was a gap wen closing a loop. Ten Zao distributed error (gap) equally in sequence to obtain a consistent map. Tis approac is direct and easy to be implemented but te accuracy is limit. Estrada.C 5 proposed a nonlinear constrained least-squares optimization approac. Te solution was found by Sequential Quadratic Programming (SQP). Tis is an impressive metod for efficient maintenance of loop consistency. However some complicated Jacobians need to be computed. Our map optimization approac was inspired by te approac proposed by Lu 6. Neverteless, we extended is approac to outdoor application and included two types of sensor data from laser radar and GPS (at te entrance and exit points of GPS signal bloced area) in te same optimization framewor. In te following, we first explore two inds of constraints and ten linearize te constraints to formulate te definition of optimal linear estimation based map optimization Constraint from range scans registration Wen veicle moves, te same pysical point p may be observed repeatedly in different veicle poses, X i, X j for example (te repeatedly observed points are called corresponding points). According to equation (), we can obtain following equation: d f( X, xv, yv ) f( X, xv, yv ) e p i i i j j j d were e d is a small value, representing errors in veicle poses and laser scan measurements. Te above equation indicates constraint on X i, X j by one scan point. If tere are points, constraint can be formulated by minimizing following expression: 2 2 dp f( Xi, xvi, yvi ) f( X j, xvj, yv j) (2) Unfortunately, because of non-linear function f, te constraint is also non-linear. In order to formulate a linear map optimization algoritm, te constraint sould be linearized. ^ ^ ^ ^ ^ ^ ^ ^ Let Xi ( xi, yi, ri ) and X j( xj, yj, rj ) be some initial estimates of X i and X j wit following equations: X X δx ^ i i i X X δx ^ j j j Measurement equation of linearized constraint is given by: D H δx H δx i, j i i j j Te observation of D i,j and corresponding covariance matrix can also be derived, wic are denoted by D i,j * and C i,j *. Note tat corresponding points used in formulating above constraint are determined by pairwise range scans registration described in section 2. Since one map frame can carry out valid pairwise registration wit several oter map frames (ere valid means registration result don t meet te conditions described in te end of section 2), we can obtain a constraints networ as displayed in Fig. 7. Fig. 7 Constraints networ Publised by Atlantis Press Copyrigt: te autors 04

6 Ming Yang, et al Constraint from GPS data In GPS signal bloced area, GPS data is unavailable. However, at te entrance and exit points of tis area, GPS data can be used to improve map accuracy. Two veicle poses corresponding to tese entrance and exit points are X 0 and X n in map frames M 0 and M n. Pose relations wit tese two veicle poses can be derived as: T g( X, X ) ( tx, ty, tr) (3) 0, n 0 n were: tx ( xn x0) cos( r0) + ( yn y0)sin( r0) ty ( xn x0)sin( r0) + ( yn y0) cos( r0) tr rn r0 Suppose te pose relation obtained by GPS data is T GPS, so we can formulate constraint as: T T e (4) GPS 0, n T were e T is a small value, representing errors wit GPS data. Tis constraint is also a non-linear one. ^ ^ ^ ^ ^ ^ ^ ^ Let X 0( x0, y0, r0 ) and X n( xn, yn, rn ) be initial estimates of X 0 and X n wit following equations: X X X ^ 0 0 δ 0 X X δx ^ n n n Measurement equation of linearized constraint is given by: D H δx H δx n,0 n n 0 0 Te observation of D n,0 and corresponding covariance matrix can also be derived, wic are denoted by D n,0 * and C n,0 *. Te result is te same wit tat of pairwise range scans registration constraint. Terefore, we can formulate a uniform map optimization framewor Map optimization wit linear constraints Combining all linearized constraints from pairwise range scans registration and GPS data, map optimization can be formulated in an optimal linear estimation sense based on maximum lieliood criterion, wic is to minimize te following Maalanobis distance: 0 < i j n * T * * i, j i, j i, j i, j i, j E ( D D ) ( C ) ( D D ) Above equation can be rewritten as: 0 < i j n * T * * ij, i j i, j i, j i j E ( D ( V V )) ( C ) ( D ( V V )) were Vi HiδX i. Furtermore, we can represent te above equation in matrix form: E D GV C D GV * T * * ( ) ( ) ( ) By applying optimal linear estimation teory, te optimal solution for V wic minimize E can be obtained: ( T ) V G ( C ) G G ( C ) D * * T * * Separating V i * from V *, we can derive following optimal veicle pose: X X δx X H V * ^ * ^ * i i i i i i After optimization, map can be updated wit new veicle poses. Note tat Taylor expansion based linearization is applied in our map optimization algoritm, in wic accuracy of linearization depends on te accuracy of initial estimate of veicle poses. Terefore, we can iteratively carry out te proposed algoritm wit new derived pose estimates to get more accurate result. Te iterative strategy converges very fast. Typically it usually taes tree or four iterations to converge to te limit of macine accuracy. 4. Veicle Localization After mapping in GPS signal bloced area, te created map can be sared wit oter veicles to localize temselves in tis area. Localization is acieved by map matcing, wic is also a pairwise range scans registration problem between current laser scan and one map frame in te map. A UKF based fusion strategy is also applied to improve reliability. Sensor data utilized in te localization are laser scans and odometry. 4.. Veicle Motion Model Fig. 8 illustrates te relationsip between veicle poses at time step i and time step i+. ICR represents Instant Center of Rotation. Veicle motion model can be derived from te geometry: xi+ xi + ds cos( ri + π 2+ dr 2) y i+ yi ds sin( ri π 2 dr 2) (5) ri+ ri + dr were (ds, dr) represents te odometry data between time step i and i+, wic can be obtained from optical encodes fixed on te veicle. Equation (5) can be used to predict veicle poses. Publised by Atlantis Press Copyrigt: te autors 05

7 Laser radar based Veicle Localization Fig. 8 Veicle motion model 4.2. Map Matcing Based Localization Te first problem in map matcing is ow to coose one best map frame as reference from entire map. To address tis problem, we use predicted veicle pose by equation (5). Te map frame wose center is closest to te center of current laser scan in world coordinates is selected. By tis idea, te selected map frame is te one wo contains te largest part of te same environmental information wit te current laser scan, wic will improve te reliability of map matcing. Because veicle pose predicted by odometry is accurate witin sort period (interval between two successive laser scans), te above idea wors well in practice. Localization is acieved by pairwise registration between te current laser scan (after preprocessing, represented by S cur ) and te selected reference map frame (represented by S ref ). Localization mainly includes tree steps: ) Project S ref to te predicted veicle pose (by equation (5)), denoted by S * ref. 2) Matcing S cur wit S * ref by pairwise registration described in section 2. Te matcing result is (tx, ty, tr), representing te translation and rotation between two scans. 3) Derive new veicle pose as: x* x cos( r) sin( r) 0 tx y* y sin( r) cos( r) 0 ty + (6) r* r 0 0 tr were (x, y, r) is te predicted veicle pose UKF Based Fusion Because of noises in data acquisition and errors in preprocessing and ICP registration, localization only by map matcing is not reliable. Terefore, we fuse map matcing result wit odometry based dead-reconing by UKF 7. UKF is a nonlinear estimation algoritm, wic utilizes a deterministic "sampling" approac to calculate te mean and covariance of te state. Te state distribution approximated by Gaussian random variables is represented by a set of cosen sample points. Tese sample points capture te true mean and covariance of te state distribution, and wen propagated troug te nonlinear system, capture te posterior mean and covariance. In our approac, system s state is veicle pose. System s dynamic model is defined as: xi+ xi + ds cos( ri + π 2+ dr 2) q Xi+ y i+ yi ds sin( ri π 2 dr 2) q (7) ri+ ri + dr q3 xi+ v Zi+ y i+ v + 2 (8) ri+ v3 were equation (7) is derived from equation (5), representing odometry based dead-reconing result. q i represents process noise wit dead-reconing in one time step, wic is modeled as Gaussian distribution wit zero mean and C q covariance. Measurement equation is expressed by equation (8), given directly by veicle pose added wit noises denoted by v i, wic is also modeled as Gaussian distribution wit zero mean and C v covariance. Observed measurement value is given by map matcing based localization applying equation (6). C q and C v can be rougly estimated offline. If te result of map matcing is trustless (meeting te conditions described in te end of section 2), C v sould be set to a very large value. 5. Experiment 5.. Experiments wit Syntetic Data A simulation system was developed to test and analyze our proposed approac. Fig.9 sows a simple syntetic environment, were polygons represent buildings, circles represent tree truns and te blac triangle represents veicle. Te veicle can be controlled troug eyboard. Configuration of laser radar was set as 80m of maximum measurement distance, 0~80 Publised by Atlantis Press Copyrigt: te autors 06

8 Ming Yang, et al degree of angular range and 0.5 degree of angular resolution. Moreover, Gaussian noises wit zero mean and standard deviation of 4cm and standard deviation of 0.2 degree were added to eac scan data point to simulate te noises may contained in real scan data. In te mapping, even small errors introduced in te veicle pose (especially in te veicle orientation) may result in large errors in a map. Here, we test te effectiveness of our mapping algoritm. Virtual veicle was controlled to move along a waved curve and veicle poses as well as laser scans were recorded for mapping. Additionally, odometry data was also generated by adding Gaussian noise to te relative veicle poses. Te true veicle trajectory and trajectory by odometry based dead-reconing are displayed in Fig.0, displaying large accumulative errors wit deadreconing. first and last true veicle poses added wit some Gaussian noise to simulate errors of GPS data) were establised. Te algoritm converges very fast. Typically it taes tree or four iterations to converge to te limit of macine accuracy. Fig. 3 displays te map created wit te new veicle poses after map optimization, wic is quite similar wit te true map. Comparing wit Fig.2, our proposed optimization algoritm acieved considerable improvement in map accuracy. Fig. 3 Optimized map Fig. 9 Syntetic environment Fig. True map Fig. 0 Veicle trajectories Fig. 2 Noisy map 5.2. Experiments wit Real Data Te proposed approac as been tested wit real data from an intelligent veicle developed at Sangai Jiao Tong University. Laser range data was captured by SICK LMS 29-S05, wit 80m of maximum measurement distance, 0~80 degree of angular range and 0.5 degree of angular resolution. Equipped GPS is a NTC2030W RTK-GPS supplied by NavCom company. Odometry data was obtained from two optical encoders equipped on driving motor and steering motor. Resolution of te former encoder is 2000 pulse/rotation and tat of te latter encoder is 000 pulse/rotation. Tese resolutions were quadrupled by DSP. Veicle speed was about 2-4m/s. Te veicle is sown in Fig. 4. True map created wit true veicle poses (scan points were transformed and displayed in te world reference frame for a better visualization) is displayed in Fig. and initial noisy map created wit veicle poses by dead-reconing is in Fig. 2, sowing considerable errors in initial map (please see te syntetic environment in Fig. 9 again for a better understanding of te mapping results). Total number of map frames was 54. Next step, we tested our map optimization algoritm. Totally 906 pairwise scans registration constraints and GPS data constraint (by using te Fig. 4 Intelligent veicle Start End Fig. 5 st experimental place Te first experimental place is displayed in Fig. 5. Veicle moved along te red pat. GPS information during tis movement is sown in Fig. 6, were (a) is te number of visible satellites and (b) is GPS woring mode (0-fix not available, -GPS fix, 2-Differential Publised by Atlantis Press Copyrigt: te autors 07

9 Laser radar based Veicle Localization GPS fix, 3-PPS fix, 4-Real Time Kinematic, 5-Float RTK). We used laser scans, odometry data as well as GPS data at start and end points to create a global map. Laser scans were recorded for mapping about every one meter along te pat. (a) Number of visible satellites optimization is displayed in Fig. 20. Optimized map created by using pairwise scans registration constraints (82 constraints) and GPS data constraint is displayed in Fig. 2, sowing good improvement in map accuracy. After a map as been created, localization can be acieved by map matcing. Fig.22 sows te localization results. Ground trut is te trajectory by GPS data fused wit odometry. Tese results prove good performance of te proposed approac. B A (b) GPS woring mode Fig. 6 GPS information along te pat Initial map witout map optimization is displayed in Fig. 7 (scan points were transformed to te world reference frame), wic contains 34 map frames. Optimized map created based on constraints from pairwise scans registration (79 constraints) and GPS data is displayed in Fig. 8. Comparing Fig. 7 and Fig. 8, we can see large improvement in map accuracy by te proposed optimization algoritm. Fig. 9 2 nd experimental place Fig. 20 Initial map Fig. 2 Optimized map of te second experiment Fig. 7 Initial map Fig. 8 Optimized map Second experiment was carried out in te place displayed in Fig.9, were veicle moved along te red pat. GPS information in tis experimental place is quite good (almost in RTK or float RTK mode) and we used GPS data as ground trut. To test our proposed approac, we assumed te region between A and B in Fig. 8 was GPS signal bloced area. Veicle localization in tis area sould be acieved by map matcing. We used laser scans (every one meter along te pat), odometry data as well as GPS data at A and B to create a map. Entire map contains 26 map frames. Initial map witout map Fig. 22 localization results Fig. 23 Effectiveness We also compared map matcing result wit tat of map matcing & UKF, displayed in Fig. 23. Because of te noises in measurements and pairwise registration, localization only by map matcing was not smoot enoug. However, map matcing fused wit odometry by UKF gave a more reliable result, sowing good effectiveness of te proposed fusion strategy. Publised by Atlantis Press Copyrigt: te autors 08

10 Ming Yang, et al 6. Conclusion GPS-based approac is popular in veicle localization. However tis approac is sensitive to surrounding environmental conditions. Many objects lie ig-rise buildings, tunnels, overead roads or even tall trees can bloc GPS signals seriously, maing localization information unavailable. If GPS signal bloced area is small, Inertial Navigation Systems or dead reconing can be integrated wit GPS to provide continuous localization information, wereas if bloced area is large, oter tecniques ave to be applied. Tis paper proposed a laser radar based map matcing approac to address te localization problem in large GPS signal bloced area. One difficulty in map matcing is ow to create a precise map. By linearizing constraints from sensor data, we designed an efficient optimal linear estimation based map optimization algoritm, wic improves map accuracy greatly. Localization is acieved by map matcing, wic is a pairwise range scans registration between current laser scan and one map frame in te map. Because of noises in data acquisition and errors in data preprocessing and pairwise registration, localization only by map matcing is not reliable. Terefore, we fuse map matcing result wit odometry based deadreconing to improve reliability. Experimental results sow te good performance of te proposed approac. Tis approac can be integrated wit existing GPSbased tecnique to provide continuous localization information. Future wor will mainly focus on te furter improvement in reliability and accuracy. Fusing wit oter sensors, lie camera, is also an empasis in our future wor. References. W. Wang, F. Hou, H. Tan, A Framewor for Function Allocation in Intelligent Driver Interface Design for Comfort and Safety, International Journal of Computational Intelligence Systems, 3(5) (200), pp J. A. Farrell and M. Bart, Te Global Positioning System and Inertial Navigation. (New Yor: McGraw- Hill, 999) 3. I. Sog, P. Handel. In-Car Positioning and Navigation Tecnologies-A Survey. IEEE Transactions on Intelligent Transportation Systems, 0(), (2009), pp W. Wang, Y. Mao, J. Jing, Driver s various information process and multi-ruled decision-maing mecanism: a fundamental of intelligent driving saping model, International Journal of Computational Intelligence Systems, 4(3) (20), pp Y. Cui and S. S. Ge, Autonomous veicle positioning wit GPS in urban canyon environments, IEEE Transactions on Robotics and Automation, 9() (2003), pp T. H. Cang, L. S. Wang and F. R. Cang. A Solution to te Ill-Conditioned GPS Positioning Problem in an Urban Environment, IEEE Transactions on Intelligent Transportation Systems, 0() (2009), pp M. Quddus, W. Ocieng, and R. Noland. Current mapmatcing algoritms for transport applications: State-ofte art and future researc directions. Transportation researc. Part C, Emerging Tecnologies, 5(5) (2007), pp D. Lee, W. Cung and M. Kim. A reliable position estimation metod of te service robot by map matcing. In te Proceeding of IEEE International Conference on Robotics and Automation, (2003), pp.2830~ G. Giorgio, et al. Fast and Accurate SLAM wit Rao- Blacwellized Particle Filters. Robotics and Autonomous Systems. 55() (2007), pp M. Yang, et al. Laser radar based real-time ego-motion estimation for intelligent veicles. In te Proceedings of IEEE Intelligent Veicle Symposium. (2002), pp K. C. J.Dietmayer, J. Sparbert and D. Streller. Model Based Object Classification and Object Tracing in Traffic Scenes from Range Images. In te Proceedings of IEEE Intelligent Veicle Symposium (200), pp D. Manandar, R. Sibasai. Veicle-borne laser mapping system (VLMS) for 3-D GIS. In te Proceedings of Geoscience and Remote Sensing Symposium. vol.5 (200), pp F. Lu, E. Milios. Robot Pose Estimation in Unnown Environments by Matcing 2D Range Scans. Journal of Intelligent and Robotic Systems. 8(3) (997) pp H. Zao, R. Sibasai, Reconstructing urban 3D model using veicle-borne laser radars. Proceedings of Tird International Conference on 3-D Digital Imaging and Modeling (200), pp C. Estrada, J. Neira, and J. D. Tardos. Hierarcical SLAM: Real-Time Accurate Mapping of Large Environments. IEEE Transactions on Robotics. 2(4) (2005), pp F. Lu, E. Milios. Globally Consistent Range Scan Alignment for Environment Mapping. Autonomous Robots, 4(4) (997), pp S. J. Julier and J. K. Ulmann. Unscented Filtering and Nonlinear Estimation. In te Proceeding of IEEE. Vol.92 (2004), pp Publised by Atlantis Press Copyrigt: te autors 09