Intelligent Multi-Sensor Measurements to Enhance Vehicle Navigation and Safety Systems

Transcription

1 Intellgent Mult-Sensor Measurements to Enhance Vehcle Navgaton and Safety Systems John W. Allen, Auburn Unversty Jordan H. Brtt, Auburn Unversty Chrstopher J. Rose, Auburn Unversty Davd M. Bevly, Auburn Unversty BIOGRAPHY John was born n Demopols, AL and grew up n Brmngham, Alabama. In 27, he obtaned hs bachelor s degree n Mechancal Engneerng from Auburn Unversty. Currently, John s worng on hs master s degree and wors n the GPS and Vehcle Dynamcs Lab. John s research nterest ncludes multsensor fuson for vehcle navgaton, and modern control theory for ground vehcles. Jordan s currently a master s student and research assstant at Auburn Unversty s GPS and Vehcle Dynamcs Laboratory (GAVLAB. He receved hs B.S. degree n electrcal engneerng n 28. Hs current research nterests nclude sensor fuson and autonomous vehcle navgaton and control. Chrstopher Rose was born n Huntsvlle, Alabama. He s a member of the GPS and Vehcle Dynamcs Lab at Auburn Unversty. Hs current actvtes focus on vdeo processng applcatons and sensor fuson n ground vehcles. He receved hs B.S. n Electrcal Engneerng at Auburn Unversty n 27. Davd M. Bevly receved hs B.S. from Texas A&M Unversty n 995, M.S. from Massachusetts Insttute of Technology n 997, and Ph.D. from Stanford Unversty n 2 n mechancal engneerng. He joned the faculty of the Department of Mechancal Engneerng at Auburn Unversty n 2 as an assstant professor. Dr. Bevly's research nterests nclude control systems, sensor fuson, GPS, state estmaton, and parameter dentfcaton. Hs research focuses on vehcle dynamcs as well as modelng and control of vehcle systems. Addtonally, Dr. Bevly has developed algorthms for navgaton and control of off-road vehcles and methods for dentfyng crtcal vehcle parameters usng GPS and nertal sensors. ABSTRACT Nearly 5% of all the traffc fataltes are due to accdental lane departures. Therefore, there s great nterest n advanced drver assstance systems to prevent unntended lane departures. The purpose of ths paper s to present a method of lane postonng that nvolves combnng a GPS/IMU navgaton system wth camera and Lght Detecton and Rangng (LDAR measurements. A dscrete Kalman flter s mplemented as the navgaton flter used to combne all the measurements. The navgaton coordnate frame s a coordnate frame attached to the road. The camera and LDAR are assumed to gve a measurement of the vehcle s current offset from the center of the current lane. Ths measurement drectly corresponds to the y-axs of the navgaton coordnate frame. The IMU used has 3 accelerometer axs and 3 gyros; however, only 2 accelerometers and gyro were used for the navgaton flter. Many vehcles come standard wth a smlar IMU set up for stablty control. Many ssues become apparent when worng wth a shftng navgaton frame. Measurements from the GPS must be mapped nto the road coordnate frame. Ths nvolves a transformaton from earth centered earth fxed ( coordnates to the road frame s coordnates. Also, the states of the navgaton flter need to be updated when shftng to the next road coordnate frame. The largest mplementaton hurdle s obtanng a lane map. Current survey technques are slow and requre road closure. There are also ssues wth how long a road frame can be wthout addng error to the system. A road frame that s too long n a turn wll cause error. The surveyed secton of road used for the paper s splt nto road coordnate frames wth an average length of around m. INTRODUCTION In order to reduce the number of traffc fataltes that occur due to unntentonal lane departures, many car

2 manufacturers are developng lane departure warnng (LDW systems that alert the drver before the vehcle departs the lane. Most of the LDW systems n producton now are solely based off of camera measurements. Camera-based LDW systems are prone to falures due to road, weather, and lghtng condtons. The purpose of ths paper s to provde a method of sensor fuson that can be used to combne measurements from a camera, LDAR, GPS recever, and IMU to contnuously measure lane poston. LDAR stands for lght detecton and rangng. These devces are avalable on some vehcles equpped wth actve cruse control. The LDAR scanner used for data collecton s an IBEO ALASCA T scanner. Ths scanner uses precse mrror movements to scan four horzontal scans n one scan cycle. The ALASCA T s capable of reportng dstance and reflectvty data, a measurement nown as echo wdth. The reflectvty data reported wll be used to dstngush lane marngs from the road s surface usng the prncple that the lane marngs should be more reflectve than the road s surface at the same dstance [6]. A more n-depth loo at how the LDAR s used to determne lane poston can be found n []. The camera used for data collecton s a standard web camera. The camera s mounted drectly below the LDAR s focal pont. The web camera taes a pcture wth every LDAR scan. The mage s thresholded to elmnate unwanted nformaton. Edge detecton and the Hough transform were employed to extract lane lnes from the mage. Ths mage s then searched usng polynomal bounds to fnd the poston of the lanes and the vehcle s lateral poston relatve to the lanes. A more n-depth loo at how the camera s used to determne lane poston can be found n [8]. In order to provde a more robust measurement of lane poston, the camera, LDAR, and GPS measurements wll be used to estmate the nertal measurement unt s (IMU bases. A dscrete Kalman flter s used to update lane poston usng the IMU, camera, LDAR, and GPS. The IMU used to collect data s a mcroelectromechancal system (MEMS automotve grade IMU. It has an update rate of 5 Hz. Therefore, the IMU can update lane poston between vson and GPS updates. Also, the IMU s able to dead recon the lane poston for a small amount of tme wthout any GPS, camera, or LDAR measurements. Fg. 2. Navgaton Flter Archtecture Fg.. IBEO LDAR Scanner wth web camera A GPS (global postonng system recever s useful for determnng poston on a global scale; however, a standalone GPS recever s not precse enough to provde an accurate poston of a vehcle wthn a partcular lane on the road. Also, GPS can only provde poston wth respect to a global coordnate frame. For ths project, poston wth respect to a lane s desred. Therefore, the only way GPS can be used for lane navgaton s to construct a map of the lane. Even when compared to a map, stand alone GPS poston s heavly based and s not as accurate as vson methods for determnng lane offset. In order to have an accurate baselne to compare results, RTK-corrected GPS poston and velocty are recorded on all data runs. The GPS recever used for data collecton s the Novatel ProPa V3. The recever s responsble for the RTK-corrected poston and velocty soluton. The Novatel s RTK-corrected poston s also used n the trac survey to obtan a lane map. All the GPS data used for the lane poston algorthm development s nondfferental (no correctons. TRACK SURVEY The proposed method of sensor fuson for lane poston requres a detaled map of the lane n the whch the vehcle s travelng. Ths s one lmtng factor n mplementaton of the proposed method. Current GPS

3 recevers for personal vehcle navgaton have a map database; however, n order to ensure accuracy, the map data base for ths algorthm needs to be precse. GPS wth RTK correctons can provde accuracy on the centmeter level; however, surveyng usng GPS can be tmeconsumng. Surveyng lanes wll also requre the road to be free of traffc. Future development of lane postonng methods could be used to bac out lane poston relatve to a nown vehcle locaton [4]. Such systems would need to be based off dfferental GPS measurements and precse atttude determnaton. Fg. 4. Plot of NCAT Trac Survey Fg. 3. Top Down Vew of NCAT Trac All the data used for ths paper was collected at the NCAT (Natonal Center for Asphalt Technology test trac n Opela, Alabama (Fg. 3. The trac s a two lane.8 mle oval wth flat straghts and 8 of ban n the corners. The trac s used to test wear on nterstate asphalt. In order to accomplsh ths, the trac s submtted to a fleet of tractor traler trucs that drve on the trac 6 hours a day, 5 days a wee. The.8 mles of asphalt are dvded nto varous segments wth each segment contanng asphalt from dfferent locatons n the Southeast and other locatons n the USA. Some areas of the trac have easly vewable lane marngs. There are areas of the trac where the mddle (dashed lane lne s mssng. The outsde lane lnes (sold cover the majorty of the trac; however, there are a few spots where there s no outsde lane lne. One football feld szed secton of the bac straght has no mddle or outsde lane lnes. Also, there s one on-ramp, one off-ramp, and one servce road ntersecton along the outsde lane of the trac. In these sectons, the outsde lane lne s mssng, and, the outsde lane lne runs off wth the off-ramp. In order to get a detaled map of the NCAT test trac, the outsde lane of the trac was surveyed. The survey was conducted wth two Novatel GPS recevers. Both of the recevers provde a narrow nteger soluton wth correctons from an on-ste base staton. One GPS recever was used to survey the mddle lane lne, and the other recever was used to survey the outsde lane lne. The center lane lne on the trac s dashed at every meters, roughly. The poston of the center of each dash of the center lane lne was surveyed. The outsde lane was surveyed perpendcular to the road at every center lane lne survey. To ad n surveyng, two survey poles were used as GPS antenna mounts. The surveyng poles provde a fxed antenna phase heght off the ground. The poles are also equpped wth a bubble to nsure the pole s held uprght. The GPS recevers used provdes measurements of the GPS antenna locaton n earth centered earth fxed ( coordnates. The tme average of the measurements was used to fnd the locaton. Tme constrants lmt the amount of tme each spot can be surveyed. We averaged s worth of data at Hz to determne the fnal poston of the survey. Each recorded poston was also corrected for the antenna heght. It s mportant to recognze that a GPS soluton wth RTK correctons s not an absolute measurement. There are consderable drfts from day to day. In order to get a more exact lane map, the poston of the lanes needs to be observed over long perods of tme. We have observed drfts n sub-meter magntudes from day to day when worng wth our RTK set-up. The relatve poston obtaned when surveyng the trac s very accurate. If, the trac s surveyed on one day usng the descrbed survey method and then surveyed agan on another day, the trac maps wll be the same but offset by some amount to the north and east. To overcome ths, the trac map can be offset by the north and east by a small amount by measurng drft at a nown locaton close to the lane. NAVIGATION FILTER A Kalman Flter [5] s the core of the proposed lane tracng estmaton algorthm. Snce the nputs to the flter are dscrete, a Dscrete Kalman Flter s used. Many tradtonal navgaton flters use the or tangental plane coordnates for navgaton. The base navgaton frame for ths project s the tangental plane. The orentaton of the plane s based off of the trac map. The navgaton frame s also called the road frame. The road frame s a coordnate frame that s attached to the road. The poston and velocty states of the flter are expressed

4 n the road frame. The headng state (Ψ s measured from the x-axs of the road frame. x y x y b ax bay b x bax ( u y b (2 ay b The navgaton flte s a 3 degree of freedom flter; therefore, the vehcle s assumed to nether ptch nor roll. Also, the vertcal poston and velocty of the vehcle s gnored. The frst assumpton does add error to the system; however, ths error s small consderng most roads are not ptched or baned at more than. poston of the pont of nterest n coordnates. Veloctes n coordnates (V can be drectly rotated nto the coordnate frame (V because the frame s not movng relatve to the frame. The base coordnate frame of the navgaton flter s a modfcaton of the frame. The data from the trac survey s used to form a map of the surveyed lane. Ths map s based off of wayponts saved n coordnates. Each waypont s located n the mddle of the lane. The wayponts were calculated usng the mdpont formula on each outsde and mddle lane lne par from the trac survey. The navgaton coordnate frame (road frame s the same as the frame, except the x-axs ponts to the next waypont the vehcle wll pass. The orgn of the frame s located at the last waypont the vehcle passed. NAVIGATION COORDINATE FRAME A tangental coordnate frame s a frame of reference that s based off of a locaton gven n longtude and lattude. For ths wor, the North East Down ( coordnate frame s defned as; x-axs pontng North, y-axs pontng East, and the z-axs s an axs that s ponted down. Headng can be measured as +/- 8 form the x-axs. The x- and y-axs of the coordnate frame le n the plane tangent to the WGS84 ellpsod. Geodetc longtude and lattude are used to descrbe the pont on the ellpsod at whch the orgn of the frame s located. Postons and veloctes expressed n the frame can be mapped nto the coordnate frame and vce versa. In order to move from the frame to the frame, the longtude and lattude of the center of the frame must be nown. Also, the poston of the center of the plane n coordnates must be nown. Equaton (3 shows the rotaton matrx used to map postons and veloctes n the frame to the frame. R sn( cos( sn( cos( cos( - Longtude of frame orgn - Lattude of frame orgn P o - Poston of frame orgn n P R V R sn( sn( cos( cos( sn( ( P P o ( V cos( sn( Poston measured n the coordnate frame can be mapped to the frame usng (4 [3]. P o s the poston of the orgn of the frame expressed n coordnates ( P o = [ x y z ]. P s the poston of the pont of nterest n coordnates; P s the (3 (4 (5 Fg. 5. Pcture of Frame Fgure 5 shows a drawng of the navgaton frame. Ψ R s the road headng. The road headng s measured from the north-axs of the frame to the x-axs of the road frame. The length of the road frame s d. Once the vehcle passes the next waypont, the road frame s shfted to a frame based on the waypont the vehcle passed. Equaton (6 s a matrx that maps coordnates n the frame to the road frame. It s based off of the angle Ψ R. R cos( R sn( R sn( cos( Multplyng the rotaton matrx from to road and the rotaton matrx from to wll result n a matrx that maps coordnates to road frame coordnates (7. R R * R R R (6

5 f ( xˆ, u (7 A total of 29 wayponts were used to construct the road frames; hence, there are 29 dfferent road frames that mae up the.8 mle trac map. Each frame has 5 peces of nformaton assocated wth t: longtude of frame orgn, lattude of frame orgn, poston of orgn n coordnates, frame headng, and frame length. All ths nformaton s needed to map measurements from coordnates. These values are also needed to determne how and when to update road frames. If each road frame s coordnates are expressed n floatng pont format, all the nformaton needed for mllon mles (28 mllon road frames can be stored n less than 8 ggabytes of dgtal storage. IMU MECHANIZATION The measurements from the IMU can be used to propagate the flters states between GPS, camera, and LDAR measurements. Each state has a dscrete tme update functon. The propagated state s a functon of the prevous state (x - and IMU measurement (u -. x y P V R xˆ ( ˆ f x, u 2 ( dt ( dt x [( u bax cos( ( u2 bay sn( ] 2 2 ( dt ( dt y [( u bax sn( ( u2 bay cos( ] 2 x ( dt[( u b u b ax cos( ( 2 ay sn( ] y ( dt[( u bax sn( ( u2 bay cos( ] b ax bay ( dt[ u b 3 ] b (8 (9 ( ( Pluggng the IMU outputs and current flter states nto ( wll result n the flter states that have been propagated (dt seconds nto the future. The state covarance matrx (P can be updated usng (2 [9]. A P Q T A s a Jacoban matrx defned by f[ ] A ˆ, j] x, u x P R ( P Po ( V (2 (3 Q s the process nose covarance matrx. Ths matrx s assumed to be a constant dagonal matrx. The values of A [ ( [ j] the dagonal elements of the process nose covarance matrx were hand-tuned to gve desred performance. MEASUREMENT UPDATE TYPES A typcal GPS/INS navgaton system has only one type of measurement update. Addng a camera and LDAR adds another type of measurement update. The camera and LDAR are assumed to gve measurements of the vehcle s current poston n the lane, and an assocated standard devaton wth that measurement. GPS The GPS measurements are recorded n coordnates. The GPS recever used to collect data also provdes an estmate of the standard devaton of the recever s poston and velocty measurement. In order to update the flter, the GPS poston must be mapped nto the road frame usng (8. The y measurement of poston provded by the GPS s dscarded. The vson methods provde a measurement of y to compensate for the GPS s y measurement. If no vson measurement s avalable, the navgaton flter becomes unobservable; therefore, the GPS y measurement must be used to mantan observablty. When no vson measurement s provded, the navgaton flter wll not provde lane level precson. The velocty measurement from the GPS can be mapped nto the road frame usng (9. The z component of the poston and velocty n the road frame are also dscarded. (4 and (5 show how to map the standard devaton of the poston and velocty n coordnates to the road coordnate frame. P R, ( P,, ( V, V R The Kalman gan s defned as [9] K P H T T ( H P H (4 (5 (6 R s the current measurement covarance matrx. The dagonal elements of ths matrx are flled n usng the standard devatons from (4 and (5 squared. H s the measurement matrx. If vson measurements are avalable, H s defned as H If vson measurements are not avalable, H s defned as H R (7 (8

6 The state and covarance matrx can be updated usng (9 and (2; where z s the measurement vector ẑ s the predcted measurement vector usng most current states. LIDAR xˆ xˆ K ( z zˆ P ( I K H P (9 (2 The LDAR wll detect and trac the lane marngs through the use of boundng, dscrmnaton, and by fndng a best match soluton to an expected value. Data taen from the LDAR was at half-angle resoluton. If multple echo wdths exsted for a sngle half-angle measurement, then those echo wdths were averaged together. Because echo wdth data from the LDAR can appear nosy at tmes due to precptaton or dead nsects on the LDAR screen, the on-road data can qucly become ndstngushable from erroneous or off-road data. To dstngush between these cases, the algorthm developed employees boundng. It establshes an area n front of the car that s approxmately two lane wdths wde. Ths wdth ensures that regardless of the vehcle s poston n the lane or durng a lane change, a lane marer can be found f one exsts. The lane marngs are found by generatng an deal scan (Fgure 6 and tang the mnmum RMS error when compared to an actual scan. The deal scan s generated by frst averagng the echo wdths located n a.6m wde area n front of the car. Ths averaged area represents the average reflectvty of the road s surface and s referred to as a baselne, whch s the area between the spes n Fgure 6. The spes n Fgure 6 represent the actual lane marngs, whch are generated by smply multplyng the baselne by a factor of.75. Fg. 6 Algorthm Generated Ideal Scan Now that the bass for algorthm-generated deal scan has been created, the RMS comparson can be performed. Ths s done by placng the leftmost sde lobe at the leftmost scan boundary that was prevously establshed and shftng t by half-angle ncrements untl that sde lobe has reached the center of the scan at zero degrees. Wth each shft of the left sde lobe, the rghtmost sde lobe s placed at the center of the scan and stretched untl the rghtmost sde lobe s equal to the rghtmost scan boundary. The algorthm-generated deal scan s stretched by smply addng n another baselne measurement between the sde lobes. To mtgate erroneous results wth each shft, the dstance between the leftmost sde lobe and rghtmost sde lobe s checed to ensure that the wdth s greater than the mnmum expected lane wdth and less than some maxmum expected lane wdth, as set forth n [2]. The mnmum RMS error generated from comparng the actual LDAR scan to the algorthm-generated deal scan s saved. The locaton of the nnermost sde lobes denote the lane angle correspondng to the lane marng. Usng that nformaton, the dstance from the center of the vehcle to the lane marng can be computed. However, f lane marngs reported by the RMS soluton are not at least.4 tmes greater than the baselne, t s assumed a lane marng does not exst. Dstance results are then run through a low-pass flter to smooth out any jumps and a weghted average where the weghts are based on the nverse of the varance for each layer s measurement. Ths smply gves preference to more consstent scans. Once these fltered dstances have been computed, our offset from center s found. CAMERA The camera s also assumed to gve the poston of the vehcle wthn ts current lane. A technque to elmnate erroneous lnes has been employed whch bounds the prevously detected 2nd order polynomal wth two other polynomals that are equdstant from the orgnal polynomal. These boundng curves employ smlar characterstcs as the orgnal curve; therefore, the vald lane marng should be detected wthn the bounded area gven smooth transtons between each frame. The effects of erroneous lnes wthn ths bounded area can be reduced by employng a Kalman flter on the coeffcents of the 2nd order polynomal. The flter also allows for smooth transtons between curved and straght roads. The measurement of the poston wthn the lane s carred out by determnng the number of pxels from the center of the mage and the estmated lane marng. Ths measurement value can then be converted to ts real world

7 equvalent and used to estmate the poston of the vehcle wthn the lane. For vson updates, (6 s used to computed the Kalman gan. R s now a x matrx, whch s equal to the varance of the current vson measurement. H s defned as H (2 Equaton (9 s used to update the state, and (2 s used to update the covarance matrx. NAVIGATION COORDINATE FRAME UPDATES Snce the navgaton frame s based off of current poston, the longtudnal poston of the vehcle n the road coordnate frame must be checed after every state update. If the longtudnal poston exceeds the length of the current road frame, then the vehcle has passed nto the next road frame. The measurements of the states are expressed n the old road coordnate frame; therefore, the states must be mapped nto the new road frame. If the vehcle has passed nto the next road frame, the frst step to update the states s to form a rotaton matrx based off of the change n coordnate frame headng between the new and old coordnate frame (22. Ψ R,+ s the headng of the new road coordnate frame and Ψ R, s the headng of the old road coordnate frame. R cos( sn( R R sn( cos( (22 The poston state vector n (24 s a 2x vector ([x, y]. d corresponds to the length of the prevous road coordnate frame. Equaton (25 shows how to map the velocty state vector nto the new road coordnate frame. vel vel vel R ( vel - Velocty n Frame ( - Velocty n new Frame ( (25 After completng the above steps, the flter s state vector wll be expressed n terms of the new road frame. Ths process must be performed every tme the vehcle moves nto a new road coordnate frame. Comparng the lateral poston state (x to d after every tme and measurement update wll ensure the navgaton flter s operatng n the approprate road coordnate frame. RTK-CORRECTED GPS POSITION AND VELCOITY BASELINE A base-lne s needed to compare the results of the navgaton flter. The base-lne used for ths paper s GPS wth RTK correctons. The RTK soluton s provded by the GPS recever. The recever uses CMR correctons broadcasted from an on-ste base staton. The corrected poston and velocty of the vehcle s saved n coordnates at 2Hz. The headng state can be updated by subtractng the change n coordnate frame headng from the current estmate of vehcle headng n the lane (23. Ψ s the estmated headng of the vehcle n the road coordnate frame. Ψ + s the estmated headng of the vehcle n the new road coordnate frame. (23 The next step conssts of updatng the poston state estmates. Equaton (24 shows how to update the poston states usng the rotaton matrx (22. pos pos pos R ( pos d - Poston n Frame ( - Poston n new Frame ( (24 Fg. 7. Lateral Poston of Vehcle Usng RTK Soluton Fgure 7 shows the lateral lane poston of the vehcle for 47 seconds of data taen at the NCAT test trac. Data from ths run s used for all of the results. The vehcle starts on the frst straghtaway facng west and then travels around the trac counter-clocwse. In addton, the vehcle accelerates from a stop to 5 mph and mantans ths speed for the remander of the lap.

8 Fg. 8. Map Plot of GPS, LDAR, Camera, and IMU Soluton wth Trac The vehcle stops after completng one lap. The vehcle never ventures more than 8 cm from the lane center. The navgaton flter has an output rate that matches the IMU s output rate (5 Hz. The RTK-corrected GPS soluton s only avalable at 2Hz. In order to quantfy the navgaton flter s error, the RTK-corrected GPS soluton s lnearly nterpolated between GPS measurements. The coordnates of poston s nterpolated based on the tme between the GPS measurements and desred measurement. These values are then mapped nto the road frame to estmate the lateral poston n the lane at the desred measurement tme. The relatvely constant bas n the GPS measurement can be seen n Fgure 9. The lateral error remans constant on straghtway, and then shfts n the 8 corners. The stand-alone GPS and IMU can not accurately estmate lateral offset n a lane [2]. Vson measurements are perfect to substtute for the lateral poston measurement. Vson measurements provde measurements that are much less based, and vson measurements have a small standard devaton [8]. RESULTS For valdaton and comparson reasons, the navgaton flter s ntally set up to blend the GPS and IMU measurements. Fgure 8 shows the trajectory of the x and y states of the navgaton flter n the coordnate frame. The soluton s heavly based due to the bases n the GPS measurement. Fgure 9 shows the lateral poston error of the GPS and IMU only navgaton flter. The blac lnes represent the wdth of a standard lane (2 ft.. Fg. 9. Lateral Poston Error for GPS and IMU only navgaton flter Fg.. Plot of GPS, LDAR, Camera and IMU Lateral Poston Estmate Compared to the GPS RTK Soluton Addng vson measurement updates to the navgaton flter greatly decreases the lateral poston estmate error. Fgure shows the lateral poston estmate from the GPS, camera, LDAR, and IMU navgaton flter. To gve some scale, the y-axs of the plot s spread out 2ft (3.66 m, whch s the average wdth of a hghway lane.

9 Fg.. Map Plot of GPS, LDAR, Camera, and IMU Soluton wth Trac The soluton never leaves the lane; therefore, the soluton s accurate enough trac a vehcle s current lane. The soluton degrades around 9 seconds due to the large secton of mssng lane lnes on the bac straghtaway. A few seconds later, the measurement drfts due to the vehcle passng by the off ramp. The lateral poston reported by the nterpolated RTK soluton s nosy n turns. Ths s due to the nterpolaton of the GPS soluton, and the nterpolaton of the curve tself. The nterpolaton of the curve s ntroduced when tryng to represent the curve n lne segments. If the curve s not splt nto enough road frames, the soluton wll vary greatly when coordnate frames are changed. If the curve s splt nto too many road frames, the soluton wll vary slghtly, but at a hgher frequency because of the hgh frequency of coordnate changng. Breang curves nto straght road frames becomes a tradeoff between how much nterpolaton nose s added and practcalty. Too many road frames wll result n ambguty as to what road frame the vehcle currently resdes. The nterpolated RTK soluton s used to solve for the error n the navgaton flter s lateral estmate. Fgure 2 shows a vast mprovement n error over the GPS and IMU only soluton. The dar lnes represent the standard wdth of a lane. CONCLUSIONS The algorthm descrbed wll provde an accurate lateral lane poston as long as lane marngs are avalable. A more n depth loo at tme to lane departure s needed to alert the drver before a lane departure [7]. The soluton drfts away qucly n areas wth poor lane marngs. Future wor wll nclude examnng ways to cut down on estmaton drft n the absence of vson measurements. Also, wheel encoders could be used to mantan the observablty of the system durng GPS outages. ACKNOWLEDGMENTS The Federal Hghway Admnstraton s fundng ths project and others across the range of ssues that are crtcal to the transportaton ndustry through the Exploratory Advanced Research (EAR Program. For more nformaton, see the EAR Web ste at cus" Fg 2. Soluton Poston Error for the GPS, camera, LDAR, and IMU REFERENCES [] J. Brtt, Lane Tracng usng Multlayer Laser Scanner to Enhance Vehcle Navgaton and Safety Systems, presented at the 29 Internatonal Techncal Meetng, Anahem, Calforna, 29. [2] J. Clanton, GPS and Inertal Sensor Enhancement for Vson-Based Hghway Lane Tracng, M.S. thess, Auburn Unversty, Auburn, Alabama, US, 26

10 [3] J. Farell, Aded Navgaton GPS wth Hgh Rate Sensors. New Yor, New Yor: McGraw Hll, 28. [4] D. Grenjner-Brzeznsa and C. Toth, "Performance Study of Hgh-End Dual Frequency GPS Recevers Tghtly Integrated wth a Strapdown INS," presented at the 2 Symposum on Moble Mappng, Caro, Egypt, Jan [5] R. E. Kalman, A New Approach to Lnear Flterng and Predcton Problems, Trans. Of the ASME-Journal of Basc Engneerng, 82 (Seres D, pp , 96. [6] J. Kbbel, Wnfred Justus, Kay Fürstenberg, Lane Estmaton and Departure Warnng usng Multlayer Laserscanner, presented at Proceedng of the 8 th Internatonal IEEE Conference on Intellgent Transportaton Systems, Venna, Austra, September 3-6, 25. [7] S. Mammar, S. Glaser, and M. Netto, Tme to Lne Crossng for Lane Departure Avodance: A Theoretcal Study and an Dxpermental Settng, IEEE Trans. Entellgent Transportaton Systems, vol. 7, pp. 2, June 26. [8] C. Rose, Vehcle Lane Poston Estmaton wth Camera Vson usng Bounded Polynomal Interpolated Lnes, presented at the 29 Internatonal Techncal Meetng, Anahem, Calforna, 29. [9] G. Welch and G. Bshop, An ntroducton to the Kalman flter, UNC-Chapel Hll, Chapel Hll, North Carolna, 26.