Advanced Sonar and Odometry Error Modeling for Simultaneous Localisation and Map Building

Transcription

1 Kleeman, Advaned Sonar and Odometry Error Modeling for Simultaneou oaliation and Ma uilding Proeeding of the IEEE/SJ International Conferene on Intelligent obot and Sytem, a Vega 3, Advaned Sonar and Odometry Error Modeling for Simultaneou oaliation and Ma uilding inday Kleeman Intelligent oboti eearh Centre, eartment of Eletrial and Comuter Sytem Engineering Monah Univerity, Autralia indaykleeman@engmonaheduau Abtrat - An advaned onar enor rodue aurate range and bearing meaurement, laifie target and rejet interferene with one ening yle wo advaned onar ytem are ued to imultaneouly loalie and ma an indoor environment uing a mobile robot hi aer reent the aroah and reult from on-the-fly ma building uing a Kalman filter and a new odometry error model that inororate variation in effetive wheel earation and angle meaurement hi model i uited to neumati tyre odometry error where the wheel earation ha been found to vary unreditably with floor urfae and ath urvature he aer alo reent tehnique for deteting onar feature lutter and eleting trong andidate for ultraoni landmar he aer illutrate that onar SAM data aoiation roblem are ignifiantly imlified when advaned onar enor are emloyed omared to Polaroid ranging module 1 Introdution Coniderable reearh effort ha been exended in roduing onar ma for loaliation and navigation of indoor environment [1,,3,4] Almot all reearh ha utilied onar Polaroid anging Module PM or equivalent that rovide range to the firt onar target he PM ha reented ignifiant obtale to roduing reliable ma due to the la of target laifiation and bearing information Claifying a feature a a oint or line tye allow redition of meaurement, allow imler data aoiation and i deirable for a oherent ma he la of reliable bearing information mae data aoiation and aurate feature loation diffiult hi aer reent an alternative aroah to onar ma building and loaliation where a high quality and aurate onar enor i deloyed he onar ytem are named advaned onar to ditinguih them from the PM hi aer reent new reult of imultaneou loaliation and ma building SAM emloying on-thefly advaned onar meaurement and two wheeled odometry Previou wor [4,6] uing an earlier verion of the onar enor required a tationary robot for onar laifiation, whih meant that meaurement were taen at interval of aroximately one metre Here the onar enor fire a the robot move whih reult in more detailed ma A new odometry model i introdued that extend reviou wor [7] and inlude the effet of random erturbation in the effetive wheel earation and i aroriate for robot with neumati wheel a in thi wor Moreover a new aroah i reented for automati alibration of odometry, eed of ound and onar oition on the robot he aer reent the eifiation of the advaned onar and robot in etion, the direted ening trategy in etion 3 and detail of the SAM aroah in etion 4 hi etion alo inlude the new odometry error model that extend [7] An automated alibration roedure i briefly deribed in etion 5 and reult are diued in etion 6 Advaned onar eifiation and SAMbot Configuration Figure 1 SAMbot with front and rear anning advaned onar SP baed enor he robot named SAMbot i hown in Figure 1 and erform on-the-fly ma building and loaliation SAMbot i ha front and rear advaned SP onar ytem, two drive wheel with enoder that reolve to 5 mm and a wheel earation of 64 mm he advaned onar ytem [8,9,4] reort range, bearing and eho amlitude out to 5 metre range with an error tandard deviation of mm and 1 degree for trong refletor uh a wall hee error deend on an aurate alibration of the eed of ound and alm air ondition he onar alo laifie target whilt the robot i moving

2 Kleeman, Advaned Sonar and Odometry Error Modeling for Simultaneou oaliation and Ma uilding Proeeding of the IEEE/SJ International Conferene on Intelligent obot and Sytem, a Vega 3, with a ingle meaurement yle of aroximately 35 me uing nearly imultaneou firing of two tranmitter Claifiation tye are lane, right-angled onave orner and edge refletor he onar ytem an moothly rotate whilt ening he an enoder reolve to 18 degree Interferene between the onar ytem and from external oure i rejeted uing a double ule heme [9,1] Sonar meaurement are time tamed by the enor to 1 me reolution and the enor lo i regularly ynhronied to the hot inux omuter lo to within me uing real time high riority thread 3 Wie direted ening trategy A imle direted ening trategy i emloyed during maing exeriment Eah onar enor ontinuouly an forward and baward aro it full angle range of 5 degree at 6 deg/e imilar to a windreen wier For the uroe of SAM, the robot motion i ontrolled manually with a joyti 31 Meaurement Seletion and Clutter For eah onar yle only one meaurement i retained for reaon of imliity and robutne he maximum amlitude meaurement i ued ine elet good ultraoni landmar After eah an, onar meaurement are lutered baed on target oition omenated by odometry A new meaurement within a mall differene to any one in a luter under ontrution i inluded in the luter he maximum amlitude meaurement of the luter i then reented to the SAM Extended Kalman filter here are two reaon that every meaurement i not roeed by the Kalman filter: i onar error are highly orrelated in time and ae [1] and o little new information would be obtained from the ame target and ii the roeing time er meaurement for SAM annot ee u with the enor data rate Cluter tatiti are et to allow rejetion of luttered luter that an aue inorret aoiation in the Kalman filter he tatiti are the deviation in range and bearing referened to the robot oition at the tart of the an he mot ommon target tye to enounter lutter i the edge, and i rejeted when the range and bearing deviation exeed 5 mm er metre range or degree er metre range 4 SAM imlementation A Kalman filter baed SAM imlementation i emloyed baed on the derition by avion [] where all ma feature are udated on eah meaurement te here are many aroahe reorted [5,4,,1,] that eed u the udate of the feature and their ovariane baed on aroximation and loal ma deomoition 41 obot tate vetor he robot tate rereent the angle and oition of the robot and the eed of ound a follow: [ θ x y ] x 1 he oition i referened to the entre of the drive wheel axi he eed of ound i etimated on-the-fly ine it inreae by about 5% er degree Celiu and i affeted to a leer extent by humidity and arbon dioxide level Over the duration of a SAM miion the eed of ound an vary ignifiantly 4 Feature ereentation hree tye of feature are identified by the advaned onar enor lane, onave orner and edge he latter two tye are rereented by a two dimenional oint x, y he lane feature i rereented by a line with an angle φ and the minimum ditane d to the global oordinate ytem origin a hown in Figure A oint on the line φ, d atifie the arametri equation: x d oφ t inφ y d inφ t oφ where t i the igned ditane from the nearet oint on the line to the origin he rereentation φ, d of a line ha the alternative of φ,- d that reult in the ame arametri equation exet t revere ign Here the two line are onidered different ine they orreond to ening from different ide of the line he angle of the line alway orreond to the onar ening diretion If the line were ened from the other ide to the oordinate origin, a negative ditane arameter would be reorded Previou onar maing wor [14] ued the line rereentation a, b to rereent a line with equation axbya b a hown in Figure hi rereentation may beome ill-onditioned when a line ae nearby the origin t max d φ Y a b t t min Figure ine rereentation he extent of a line i rereented by maximum and minimum t arameter t max and t min from equation X

3 Kleeman, Advaned Sonar and Odometry Error Modeling for Simultaneou oaliation and Ma uilding Proeeding of the IEEE/SJ International Conferene on Intelligent obot and Sytem, a Vega 3, New onar meaurement are only fued with the line if their t arameter lie in the interval [t max ext, t min -ext] where 4 metre ha been hoen for ext Similarly oint feature rereent an extent baed on viewing angle whih are retrited to le than a 9 degree range er feature to aount for the roertie of orner and edge in ratie In artiular the ourrene of orridor wall moulding hown in Figure 3 are laified by the onar a an edge he moulding rotrude a entimetre from the wall whih i not uffiient to allow the formation of a virtual image of the onar tranduer Neverthele thee feature are trong aouti refletor y retriting a ingle feature to 9 degree viewing angle, the left and right edge in Figure 3 are revented from fuing in the SAM algorithm o hel with maintenane of ma feature, the number of enor meaurement aoiated with a feature i reorded along with the time of thee meaurement hi an be referened to an array of time lined to robot oe Sonar laifie a edge Figure 3 Plan view of a wall moulding laified a a onar edge 43 Odometry error model he robot odometry etimate of robot oe at time te 1 i determined uing the following [7] equation: f x θ y [ x y ] [ x 1 x f x, y, y Sonar laifie a edge ] 3 o θ in θ x 4 where at time te, θ i the robot heading angle, and x and y are the x and y oordinate of the entre of the robot wheel axi he error model adoted here aume the error oure are additive white noie on the wheel earation,, and the left and right wheel length meaurement, and hi wor extend reviou wor [7] that onider noie on and alone he robot emloyed in the urrent wor ha wide inflatable tyre that rovide good tration at the exene of wheel earation variation Calibration exeriment have hown that the effetive wheel earation varie with floor urfae and ath urvature in an unreditable manner Uing a firt order aylor exanion of equation 3,4 the iterative equation an be derived in term of the Jaobian and f : x 1 I x y 5 where 1 I inθ 1 6 oθ oθ inθ oθ inθ 7 inθ inθ oθ inθ oθ oθ θ θ 8 he ovariane of the error in the robot oe at te 1 i now derived in term of te : P 1 E x 1 x 1 I E x E y y P P P P Q new error ov ariane roagation x I where E i exetation and Q i the error ovariane of y, the wheel meaurement and earation Note that in equation 9 the error in robot oe, x, at time and the new error in odometry arameter y at time are aumed to be indeendent he 4 term involving P in the equation 9 an be redued to the following equation that are effiient to imlement: 9

4 Kleeman, Advaned Sonar and Odometry Error Modeling for Simultaneou oaliation and Ma uilding Proeeding of the IEEE/SJ International Conferene on Intelligent obot and Sytem, a Vega 3, where P [ ij ] and oθ and inθ Note that if the above equation are imlemented in the order hown, the ovariane entrie an be udate in itu he rime indiate that the remaining term involving Q in equation 9 need to be inluded before the final ovariane P i obtained he error in, and are aumed to be indeendent: Q Exanding the final term in equation 9 reult in the ovariane udate equation: How are value determined for the variane? For a given ditane or angle hange, the final ovariane hould be indeendent of the number of te to travere a ath o find a value for, onider the and, ie angle variane additive term in the above equation when hi term mut be roortional to the abolute value of the angle hange in order to rodue a onitent um for different te ize herefore a roortionality ontant i introdued to give: et A be the angle error tandard deviation introdued by noie on the wheel earation arameter for a full revolution of the robot in one te, then A therefore and A A 14 Value for and mut be roortional to at eah te o that the total ontribution of error along the ath i deendent only on the ath not the number of te o aumulate a tandard deviation error E from 1 metre of travel, E and E 15 eite the aarent omlexity of the ovariane udate equation 1, they are traightforward to imlement and the C ode to udate the odometry oe and ovariane run in under 5 ue on a 85 MHz Pentium hi rereent about 5 % omutational loading ine the odometry etimate and error ovariane i udated every 1 me 44 Sonar error ovariane model he onar error model etimate the ovariane of error in onar range and bearing meaurement he error model deend on the feature tye Meaurement of edge are le reliable than lane and orner feature tye, due to two fator: i redued eho amlitude and ii lutter i more ommon herefore a more onervative error model i adoted for edge feature he range and bearing meaurement are aumed to be indeendent [15] he onar error model need to tae into onideration vibration and waying of the roboti latform that are not een by odometry hee error an be ignifiant omared to the tationary robot error and the onar model adoted i neearily onervative: [ ] / 18 1 * range metre degree ale eond OF radian angle z 16

5 Kleeman, Advaned Sonar and Odometry Error Modeling for Simultaneou oaliation and Ma uilding Proeeding of the IEEE/SJ International Conferene on Intelligent obot and Sytem, a Vega 3, where ale i maximum{1,rangein metre} for edge and 1 for orner and lane 45 Meaurement to Feature Aoiation New meaurement an either generate a new ma feature or be fued to exiting ma feature In the latter ae the meaurement imrove the robot oe and the auray of the feature oe and oibly inreae the extent of the feature In the Kalman filter all error ovariane and ro ovariane matrie are affeted by the fuion of a meaurement and the roeing omlexity of the oeration inreae with n unle the ma i divided into ubma [5,4,] he deiion of whih feature, if any, a meaurement hould be fued with i alled the aoiation roblem Inorret aoiation an aue gro error in the ma and even divergene of SAM Failure to aoiate a meaurement with it genuine feature reult in ubotimal robot loaliation and feature error and oibly the duliation or hadowing of future ma feature dilaed in oition and angle Aoiation mut atify: 1 he feature and meaurement laifiation agree ie lane, orner or edge he length of a line i extended by le than 4 metre 3 For a oint feature, the viewing angle to the entre of reviou viewing angle i le than 3 degree 4 he meaurement atifie a tandard validation gate ondition for a Kalman filter [14] for one and only one feature If one of the ondition 1, or 3 are not atified and the meaurement doe not fall into any validation gate, a new feature i initiated If the meaurement fall into more than one feature validation gate, the meaurement i ignored 5 Calibration of Odometry and Sonar he neumati tyre require odometry alibration to be erformed frequently Calibration of the onar oition in the robot oordinate frame and the eed of ound i a byrodut of thi tehnique Sonar and odometry meaurement are olleted for a robot maneuver that allow both onar enor to oberve a dozen unluttered feature over a few minute with the robot moving forward turning and retraing it ath everal time a hown in Figure 4 In order to determine the eed of ound and rovide a ditane referene for odometry, the ditane between two arallel lane i meaured and ued a a ontraint during minimiation of a ot funtion he ot funtion i the um of quare differene between exeted meaurement and real meaurement baed on average feature oition and odometry Seial are i taen in hooing the form of the arameter and their order to deoule the minimiation of ot between arameter For examle i otimied firt ine thi affet onar feature erendiular to robot motion, and then / whih affet feature in line with robot motion only Other arameter of odometry and onar follow: Figure 4 he robot ma data ued for alibration of odometry, eed of ound and onar oe on the robot 6 eult Figure 4 and 5 how the reult of SAM exeriment where the robot wa travelling at le than 1 m/ in order to rodue dene ma hee ma an be roeed in real time ine le than feature tyially are reent Meaurement innovation rodued by the Kalman filter are uually le than a 1 m and 1 radian exet when feature not een for a oniderable time are enountered In figure 5, hantom feature are oberved outide the hyial orridor due to multi-ath ehoe hee do not reent a roblem for SAM unle the ma i ued for navigation In that ae tehnique exit to allow detetion of thee feature [6] 7 Conluion and Future Wor wo advaned onar ytem ombined with a new odometry error model have been hown to rodue high quality onar feature ma with a Kalman filter SAM aroah hi wor highlight the benefit of aurate and aroriate enor information that irumvent higher level ohitiated hyothei formation over extended time eriod that i neeary to aoiate and laify with onventional Polaroid ranging module onar

6 Kleeman, Advaned Sonar and Odometry Error Modeling for Simultaneou oaliation and Ma uilding Proeeding of the IEEE/SJ International Conferene on Intelligent obot and Sytem, a Vega 3, [3] he advaned onar allow reliable meaurement aoiation with ma feature Neverthele the aoiation an fail when odometry error exeed exetation or lineariation error aumulate a reorted in [] and future wor will onentrate on imroving aoiation and reovering from failure of SAM he related roblem of loo loure and laer onar fuion will alo be invetigated eferene [1] ubouhi; Nowaday trend in ma generation for mobile robot IOS 96, : 88 8 [] G Zunino,; H Chritenen, Simultaneou loalization and maing in dometi environment Int Conf Multienor Fuion and Integration for Intelligent Sytem, 1 : 67-7 [3] H J S Feder, J J eonard and C M Smith Adative mobile robot navigation and maing IJ, July [4] JJ eonard, HJS Feder, eouled tohati maing for mobile robot & AUV navigation IEEE J Oeani Eng, Vol6, I4, 1 : [5] K S Chong and Kleeman, Feature-baed maing in real, large ale environment uing an ultraoni array, IJ, 1999, 3-19 [6] K S Chong and Kleeman, Sonar baed ma building for a mobile robot,, IEEE Int Conf oboti & Automation, 1997, [7] K S Chong and Kleeman, Aurate odometry and error modelling for a mobile robot, IEEE Int Conf oboti & Automation, 1997, [8] Kleeman, On-the-fly laifying onar with aurate range and bearing etimation IOS, [9] A Heale and Kleeman, Fat target laifiation uing onar IOS 1, [1] Kleeman, Fat and aurate onar traer uing double ule oding, IOS 99, 85-9 [] A avion, Mobile obot Navigation Uing Ative Viion Phil hei Univerity of Oxford, 1998 [1] M Montemerlo, and S hrun, and Koller and Wegbreit, FatSAM: A Fatored Solution to the Simultaneou oalization and Maing Problem,, Pro of the AAAI National Conf Artifiial Intelligene, Edmonton, Canada [] J Guivant, E Nebot Imroving Comutational and Memory equirement of Simultaneou oalization and Ma uilding Algorithm Pro IEEE Int Conf oboti & Automation, [14] K S Chong and Kleeman, Mobile robot ma building for an advaned onar array and aurate odometry, IJ 1999, -36 [15] Kleeman and Ku, "Mobile robot onar for target loalization and laifiation", IJ 1995, Figure 5 Ma and meaurement for a orridor where the robot travel u 9 metre turn and travel ba to the tart oint Ma feature are labelled with a equene number and number of aoiation Meaurement are indiated by a line from the robot oition to the meaurement oint