Stereo Vision Based Navigation of a Six-Legged Walking Robot in Unknown Rough Terrain

Transcription

1 Sereo Vision Based Navigaion of a Six-Legged Walking Robo in Unknown Rough Terrain Anne Selzer, Heiko Hirschmüller, Marin Görner Absrac This paper presens a visual navigaion algorihm for he six-legged walking robo DLR Crawler in rough errain. The algorihm is based on sereo images from which deph images are compued using he Semi- Global Maching (SGM) mehod. Furher, a visual odomery is calculaed along wih an error measure. Pose esimaes are obained by fusing inerial daa wih relaive leg odomery and visual odomery measuremens using an indirec informaion filer. The visual odomery error measure is used in he filering process o pu lower weighs on erroneous visual odomery daa, hence, improving he robusness of pose esimaion. From he esimaed poses and he deph images, a dense digial errain map is creaed by applying he locus mehod. The raversabiliy of he errain is esimaed by a plane fiing approach and pahs are planned using a D* Lie planner aking he raversabiliy of he errain and he curren moion capabiliies of he robo ino accoun. Moion commands and he raversabiliy measures of he upcoming errain are sen o he walking layer of he robo so ha i can choose an appropriae gai for he errain. Experimenal resuls show he accuracy of he navigaion algorihm and is robusness agains visual disurbances. 1 Inroducion Research on auonomous robos has gained more and more imporance over he pas years. Following he recen naural disasers and nuclear hreas, he public desire for auonomous robos o suppor humans has grown significanly. In applicaions such as search and rescue and planeary exploraion mobile robos would be valuable o preven humans from being exposed o danger. In such applicaions, robos have o deal wih rugged errain, seep slopes and changing subsraes. Under hese condiions, walking robos are expeced o show superior performance o wheel driven ones. Their advanages are, no need for pahs of coninuous conac wih he ground, he abiliy o sep over or on obsacles as well as o climb various rock formaions. However, walking robos suffer from limied payloads, which prevens hem from carrying heavy insrumens. Thus, using a heerogeneous eam of robos seems o be a promising soluion. Such a eam could consis of large wheeled robos for supply and long disance ranspor, aerial robos for overviewing he errain and a eam of highly mobile muli-legged robos for local search and exploraion asks. Before ackling he problem of muli-robo cooperaion, firs he imporan challenges of robus locomoion and robus auonomous navigaion need o be solved. 1

2 We believe, ha each robo in a eam should be able o fulfill is ask independen of he oher eam members. This implies ha each robo carries all necessary sensors o perceive is own sae as well as is immediae environmen. If available, i should use informaion provided by oher robos, such as map pars or localizaion daa, bu i should no be relian on i. Furhermore, i should be independen of a priori maps and exernal infrasrucure such as GPS, bu i should be able o benefi from such informaion when accessible. The independence of oher eam members is an imporan feaure since he failure of a single robo mus no compromise successful ask compleion. In his paper we will focus on muli-legged robos as rough errain specialiss in a heerogeneous robo eam. In our opinion a sereo camera is he appropriae sensor for perceiving he environmen. I is ligh weigh, versaile as well as passive and can be used for moion esimaion and creaing geomerical models of he environmen. Furher, is daa could in fuure enable cogniive processes relevan o he ask. Addiionally, for he walking ask iself, a legged robo already embeds many propriocepive sensors, such as join angle sensors and join orque sensors. Besides delivering informaion abou he inernal sae of he robo, hey are also a valuable source of informaion abou he environmen. Thus, including foo force sensors and an inerial measuremen uni (IMU), he percepion of a walking robo is mainly based on a combinaion of visual, inerial and acile informaion. Some of hese sensors give redundan informaion which can be combined o improve he daa qualiy and which can also enhance robusness agains sensor failure. We believe ha a legged exploraion robo should use a layered conrol archiecure [4] consising of a walking layer, a navigaion layer and a highlevel ask planner. The walking layer is responsible for he basic asks of sable sanding and walking, bu also provides reflexes for non-fla errain o overcome obsacles wihin he walking heigh reacively. For his purpose, he walking layer mainly uses he propriocepive sensors. The ask of he navigaion layer is o guide he robo along an opimal pah o a goal poin using visual informaion of he environmen. In order o gain he larges benefi from visual and acile daa, boh layers should be srongly coupled. On he one hand, he navigaion layer should provide high-level moion commands and informaion ha helps he walking layer o anicipae upcoming errain. On he oher hand, he walking layer should send curren moion informaion and acile clues abou he ground o he navigaion layer. Furher, i should provide informaion abou he curren moion capabiliy of he robo. For example, if he robo is heavily loaded or damaged, he navigaion layer needs o reac by adaping he pah. On he op level, a ask planner needs o generae asks in order o fulfill he mission goal and o handle excepions. For example, if he navigaion layer repors ha he desired goal poin canno be reached, he ask planner has o choose a differen goal poin. The objecive of his paper is o presen a visual navigaion algorihm for he six-legged walking robo DLR Crawler (ref. Fig. 1). This robo is a prooypic sudy for fuure exploraion robos. I serves as a es plaform o gain experience in walking and navigaion as a sep owards an auonomous walking robo in a heerogeneous eam of robos. The DLR Crawler implemens a walking layer using differen gai algorihms and reflexes which enable i o overcome obsacles wihin is walking heigh reacively. The navigaion algorihm generaes moion commands for he walking layer o lead he robo along 2

3 Figure 1: The DLR Crawler wihin he gravel esbed a shor and safe pah o a goal poin. I is designed for unsrucured errain and does no depend on exernal infrasrucure such as GPS, nor a priori maps of he environmen are required. Only measuremens from an on-board IMU, leg odomery and he sereo camera are used o auonomously find and follow a pah o given goal poin coordinaes. The main poins his paper discusses are: Sereo visual odomery algorihm wih error esimaion Mulisensor daa fusion of inerial, leg odomery and visual odomery measuremens for robus pose esimaion using an indirec informaion filer Dense elevaion mapping from sereo dispariy images compued by Semi- Global Maching (SGM) Esimaion of he errain raversabiliy according o he moion abiliies of he robo Pah planning considering he esimaed errain raversabiliy and he curren sae of he robo Experimenal resuls showing he accuracy of he navigaion algorihm as well as is robusness agains visual disurbances The paper is organized as follows: Secion 2 will provide a brief overview on relaed work in visual navigaion. Secion 3 inroduces he DLR Crawler, describing he hardware as well as he implemened walking algorihms and reflexes. In Secion 4 an overview of he visual navigaion sysem is given. Secion 5 describes he sereo image processing and he visual odomery calculaion. The sensor daa fusion algorihm for pose esimaion is presened in Secion 6. Secion 7 deals wih mapping rough errain and Secion 8 describes he raversabiliy esimaion. Pah planning and moion conrol are presened in Secions 9 and 10. In Secion 11 he on-line implemenaion of he navigaion algorihm is explained briefly. Experimenal resuls of he visual navigaion sysem are shown in Secion 12. Finally, Secion 13 concludes his paper. 3

4 2 Relaed Work In lieraure, several papers address auonomous rough errain navigaion. One of he firs sysems which was able o auonomously navigae on rough errain was he Auonomous Land Vehicle (ALV) developed a he Hughes Arificial Inelligence Cener in 1987 [8]. I used a laser scanner o build a navigaion map and marked areas which would cause invalid vehicle configuraions as unraversable. NASA s Je Propulsion Laboraory developed he Rocky rovers, which were es plaforms for mosly sensor based navigaion algorihms [30, 27]. They were prooypes for he Sojourner Mars rover, which arrived on Mars in Using a camera and a laser sriper, i was able o deec and avoid obsacles along is way [31]. However, hese sysems only used a binary represenaion of he environmen consising of obsacles and free space. The RANGER navigaion sysem [23] is able o esimae he difficuly of raversable areas by compuing he configuraion a vehicle would have on cerain errain poins. This knowledge is hen included ino he pah planning process. Based on RANGER he Morphin algorihm [35] was developed. Using sereo images, i esimaes he raversabiliy of regular sized errain paches by fiing a plane o he daa. Morphin uses he raversabiliy measures of hese paches o evaluae he safey of possible seering arcs for he rover. These arcs are furher evaluaed on heir use for reaching he specified goal poin. The arc wih he highes oal voe is hen commanded o he rover. Morphin provided a basis for he developmen of he navigaion sofware of he Mars exploraion rovers Spiri and Opporuniy. Visual odomery, wheel encoder readings and IMU daa are combined for posiion esimaion [28]. The auomaic navigaion mode uses he local pah planner GESTALT [12]. By fiing a plane o a local errain pach, he raversabiliy of ha grid cell is esimaed and seering angles are commanded which lead he rover along a safe pah in he direcion owards he goal poin. Since GESTALT is a local pah planner an addiional global Field D* planner was implemened [5]. Konolige e al. presened a sereo vision based oudoor navigaion sysem using IMU, GPS and visual odomery [25]. Visual odomery, wheel encoder readings, IMU daa and GPS are used for posiion esimaion. A ground plane is exraced from he sereo deph images and obsacles are deeced by hresholding he heigh above he ground plane. A modified gradien planner is used o plan global pahs. The four-legged robo BigDog implemens a gai conrol sysem and an auonomy and percepion sysem [38]. I uses propriocepive force and posiion sensors, as well as GPS, an IMU, a sereo camera and a LIDAR scanner for navigaion. Visual odomery, leg odomery and IMU daa are used for pose esimaion. Obsacles are deeced using LIDAR and sereo vision and a 2D cos map is creaed. There is no raversabiliy esimaion of he errain bu cell coss are compued from obsacles and disances o obsacles. Using he cos map, pahs are planned using an A* planner. 4

5 3 The DLR Crawler The DLR Crawler is a prooypic, six-legged, acively complian walking robo ha is based on he fingers of DLR Hand II. I is a sudy for fuure exploraion robos ha is inended o be used as laboraory esbed for he developmen of gai and navigaion algorihms. Following, a brief overview of he hardware and he conrol algorihm is given while a deailed descripion can be found in [14] and [15]. 3.1 Hardware The DLR Crawler has a oal mass of 3.5 kg and is fee span an area of 350 mm 380 mm. Each of he six legs has four joins and hree acive degrees of freedom. All joins are driven by permanen magne synchronous moors in combinaion wih harmonic drive gears and a ooh bel ransmission. The Crawler hoss a variey of propriocepive sensors. Wihin each join hese are, a moor angle sensor, a link side join angle sensor as well as a join orque sensor. Addiionally, each foo hoss a 6 degrees of freedom (DOF) force-orque sensor and he body implemens an IMU. For he purpose of visual odomery and vision based navigaion a sereo camera head is mouned. Since he robo is a laboraory esbed, all conrol compuaion is done exernally on a QNX realime PC while he navigaion algorihm employs an exernal Linux compuer. This allows o quickly es differen algorihms wih varying compuaional complexiy wihou caring abou opimized implemenaion on specific on-board hardware a his sage. Furher, he robo has an exernal 24 V power supply and on-board power disribuion. 3.2 Walking Algorihms and Reflexes Two differen gai algorihms are implemened on he Crawler. They differ no only in heir capabiliy bu also in heir compuaional complexiy. The firs gai is a ripod for moderae errain wih an underlying fixed coordinaion paern requiring lile compuaional power. The second gai is based on a biologically inspired variable coordinaion and muliple reflexes. This gai is more complex bu allows o handle more challenging errain and is even able o handle leg loss. In order o auonomously and sably negoiae mid-size obsacles and holes which are wihin he walking heigh of he Crawler, reacive behaviors are needed. These are differen reflexes ha adjus he posure of he Crawler or reac o collisions during sepping moions. The firs reflex of he Crawler is he srech reflex. The purpose of his reflex is o enforce he ground conac during he power sroke of a leg. If afer a sep he leg does no hi ground a he anicipaed heigh or he leg looses ground conac due o a rolling sone, he reflex ges acivaed and ries o find conac by quickly exending he leg. I is riggered using orque hresholds of he proximal and medial joins and is swiched off if he leg achieves a cerain load or reaches some kinemaic limi. The second reflex is he elevaor reflex ha is riggered once a sepping leg his an obsacle. This reflex moniors all join orques and ges acivaed afer some hresholds are passed. In his case i reracs and raises he leg in order o surpass he obsacle. 5

6 The combinaion of hese reflexes allows he Crawler o auonomously negoiae mos obsacles wihin is walking heigh and o adap o rough and uneven errain. The elevaor reflex requires a flexible gai coordinaion since is execuion by a sepping leg causes exended power sroke phases in he supporing legs. Thus, all reflexes ogeher wih he biologically inspired gai give he Crawler he highes capabiliy o maser rough errain. Neverheless, he use of he ripod paern ogeher wih he srech reflex is a good opion for easy errain due o is achievable speed and low compuaional complexiy. 4 Navigaion Algorihm Overview Auonomous navigaion requires a robo o coninuously esimae is curren posiion in he environmen and o plan and follow a pah o a predefined goal poin. An overview of he navigaion algorihm for he DLR Crawler is shown in Fig. 2. I uses he IMU, leg odomery and he sereo camera running visual odomery for accurae and robus pose esimaion. The esimaed pose and he deph images compued from he sereo daa are used o build a 2.5D errain map. The raversabiliy of he errain is esimaed from he map and a safe and shor pah is planned o he goal poin aking he moion capabiliies of he robo ino accoun. Moion commands for following he pah and he esimaed difficuly of he errain are sen o he walking layer. The single blocks will be described in deail in he following secions. Sereo Vision (5) Visual Odomery IMU Deph Image Pose (6) Esimaion Leg Odomery Mapping (7) Curren Pose Moion Capabiliy Terrain Difficuly Map Travers. (8) Esimaion Travers. Map Pah (9) Planning Pah Moion Conrol (10) Moion Commands Walking Layer Figure 2: Overview of he navigaion algorihm. Secion numbers are given in brackes. 5 Sereo Vision The sereo camera head consiss of wo AVT Guppy F080B grayscale firewire cameras. They conain a CCD chip wih a resoluion of pixel. The cameras are equipped wih Theia wide angle lenses wih a focal lengh of 1.3 mm. Tha resuls in a horizonal opening angle of abou 123. Sample images can be found in secion 12. The sereo baseline is abou 50 mm. 6

7 The cameras are calibraed a full resoluion and afer ha he images are downscaled o pixel which is sufficien for his applicaion and saves compuaion ime. The images are recified by projecing boh images ono a common plane ha has he same disance o boh opical ceners [18]. Recificaion enforces ha he projecion of any feaure appears in he same image row in boh images. This saves processing ime of he following seps. 5.1 Sereo Maching Dense sereo maching is performed for compuing deph images of he scenery in fron of he robo. A correlaion based approach [20] is normally sufficien for obsacle avoidance and navigaion. However, i has been found ha using Semi-Global Maching (SGM) is more advanageous since i delivers denser resuls wih far fewer ouliers [19]. Furhermore, SGM can also reconsruc hin or small objecs ha are ofen undeeced by correlaion mehods. For hese reasons, SGM has also been used in a real-ime FPGA implemenaion for driver assisance asks [11]. SGM is based on he idea of pixel-wise maching, suppored by a global smoohness consrain. The resuling global cos funcion is minimized along 8 pah direcions ha originae a he image border. Thus, 8 pahs mee a every pixel. They are combined by summing heir coss and he dispariy ha minimizes he cos is chosen for each pixel separaely. Occlusions and mismaches are idenified and invalidaed by a lef/righ consisency check ha inverses he roles of boh cameras and removes all dispariies ha differ. Census [39] has been chosen as maching cos, due o is robusness agains many radiomeric changes [22]. For real-ime performance, an implemenaion on he GPU has been used. The original implemenaion [10] has been exended for supporing Census as maching cos. The GPU implemenaion runs wih 4 5 Hz on VGA sized images ( pixel) wih 128 pixel dispariy range on a GeForce GTX 275. The runime depends linearly on he number of pixels and dispariies. In his applicaion, on images wih pixel wih 128 pixel dispariy range, a rae of abou 6 Hz is achieved. A cusomized FPGA implemenaion which enables on-board processing for he DLR Crawler is anicipaed in fuure. 5.2 Visual Odomery Visual odomery is he deerminaion of he camera movemen wih respec o he environmen. Sereo camera mehods permi compuing all six degrees of freedom (i.e. ranslaion and roaion) in conras o mono camera mehods ha can only deermine he direcion, bu no he scale of moion. Visual odomery is independen of wheel or leg slip, bu i assumes a (mosly) saic environmen. Since he anicipaed frame rae is raher low, large moions can occur beween consecuive images, especially if he robo roaes, i.e. urns lef or righ. Therefore, a mehod has been chosen ha does no rely on feaure racking [21, 18]. Fig. 3 shows an overview of he mehod. Feaure poins in consecuive lef camera images are seleced by he Harris corner deecor [17]. A square pach around each corner is used as feaure descripor. The descripor is exraced from Rank ransformed [39] lef images for making he comparison of descripors wih he sum of absolue differences 7

8 Rec. lef image Corner deec. Se of 2D poins Calculaion of 3D pos. Se of 2D/3D poins Iniial correspondences Rec. righ image Sereo maching Dense dispariy image Se of corresp. 3D poins Oulier deecion Se of 2D/3D poins from previous image Subse of consisen, corresp. 3D poins Calculaion of ransformaion Rigid camera moion Figure 3: Overview of he used sereo visual odomery mehod (SAD) robus agains radiomeric changes. The Rank ransformaion is slighly inferior o Census, bu can be compued faser. I has been found ha using such a non-parameric ransformaion is also robus agains small roaions and perspecive changes, since hey are oleran agains ouliers, in conras o classical correlaion mehods like normalized cross correlaion. I should be noed ha using scale and roaion invarian feaures such as SIFT is no beneficial, since roaion around he opical axis will be minimal as well as scale differences. In conras, using roaion and scale invarian descripors can lead o worse resuls due o less discriminaive power. All corners of one image are compared agains he corners of he previous image for finding iniial correspondences. Thereafer, he corners of he previous image are compared o he corners of he curren image and only hose correspondences ha agree in boh direcions are reained. All of he corresponding feaure poins are reconsruced in 3D in heir own camera coordinae sysem using he deph image from sereo maching. I is assumed ha here are many wrong correspondences. For finding hese correspondence ouliers, he saic scene assumpion is used. If he scene is saic, hen he 3D disance of wo poins in he previous camera coordinae sysem mus be he same as he disance of he corresponding poins in he curren camera coordinae sysem. For comparing disances, i is very imporan o ake sereo reconsrucion errors ino accoun. Le x 1, y 1, z 1 and x 2, y 2, z 2 be wo reconsruced feaure poins in he same camera coordinae sysem. The disance beween boh is obviously L = (x 1 x 2 ) 2 + (y 1 y 2 ) 2 + (z 1 z 2 ) 2. (1) An assumed uncerainy of ε p = 0.2 pixel in he image plane propagaes ino an uncerainy in he lengh by ε L = ε p z1 2 Lf (A + B + C) + z2 2 (D + E + F ), (2) A = ((x 1 x 2 ) ( x 1 ) (y 1 y 2 ) y 1 (z 1 z 2 ) z 1 ) 2, (3) B = ((x 1 x 2 ) x 1 + (y 1 y 2 ) y 1 + (z 1 z 2 ) z 1 ) 2, (4) C = 1 2 ( (y 1 y 2 )) 2, (5) D = ((x 1 x 2 ) ( x 2 ) (y 1 y 2 ) y 2 (z 1 z 2 ) z 2 ) 2, (6) 8

9 E = ((x 1 x 2 ) x 2 + (y 1 y 2 ) y 2 + (z 1 z 2 ) z 2 ) 2, (7) F = 1 2 ( (y 1 y 2 )) 2, (8) wih f as focal lengh in pixel and as baseline, i.e. disance beween boh cameras. The lengh L 1 can be considered equal o L 2, if L 1 L 2 3 ε 2 L 1 + ε 2 L 2. (9) If his consrain is violaed, hen a leas one of he correspondences mus be wrong. Oherwise, one or boh may be correc. I appears ha finding he larges subse of corresponding poins ha saisfies he consrain for all pairs, is an NP problem. Therefore, no he larges, bu jus a large consisen subse is deermined. All combinaions of corresponding pairs are compared and sored. The consisen subse is consruced by saring wih he poin ha is consisen o he mos oher poins. Furher poins are incremenally added, if hey are consisen wih all poins ha are already in he consisen subse, by preferring he poins wih he highes number of furher consisencies. This process ends if here are no more poins ha are consisen wih all oher poins of he se. This mehod is similar o a random sampling approach (RANSAC), as i finds a large oulier free subse by assembling i boom-up from a small consisen subse. However, in conras o RANSAC, he mehod is deerminisic as no form of randomizaion is used. I should be noed ha here are oher mehods for finding a large oulier free se of correspondences, ha could also be used [2, 32]. However, he mehod described above has proven o be well suied for his applicaion because i is robus and has low compuaional coss. The rigid moion is deermined as he roaion and ranslaion beween corresponding reconsruced poins. I is compued in closed form by singular value decomposiion as shown by Haralick e al. [16]. A reamen for he special case wih planar poin ses is shown by Arun e al. [1]. Wih his iniial resul, Chauvene s crierion is used o idenify and eliminae all correspondences, wih unexpeced high errors. The rigid moion is paramerized as x = ( ) T x y z n 1 n 2 n 3, wih as ranslaion vecor, n as roaion axis and α = n as roaion angle, wih 0 α π. Wih he iniial moion, corresponding poins can be reconsruced and he reprojecion error vecor y = ( ) p 1x p 1x... p T ny p ny compued as he difference beween feaure poin locaions p i in he image and he corresponding projeced locaions p i of heir reconsrucions. Alernaively, he error vecor can be compued using he ellipsoid error model [29], which is a very good approximaion, bu faser o compue and herefore preferred. The funcion o be minimized (e.g. by Levenberg-Marquard) is y = f (x) wih y 0 = f (x 0 ) as soluion wih he lowes reprojecion error. 5.3 Visual Odomery Error Esimaion The original publicaion of he visual odomery mehod [21, 18] has been exended by esimaing he moion error as well. This error depends no only on he number, bu also on he disribuion of feaure poins in he image. Abou ε p = 0.5 pixel is a ypical error in feaure poin localizaion (i.e. in he error vecor y), assuming ha he correspondences are oulier free. The propagaion 9

10 of his error ino he parameers x resuls in he parameer error ε x. For compuing his error, he funcion f (x) mus be invered. Since i is no inverible in closed form (oherwise a non-linear opimizaion would no be needed), a linearizaion is compued a x 0 as approximaion, y = f (x) J 0 (x x 0 ) + y 0, (10) wih J 0 as Jacobian marix a he soluion x 0. The Levenberg-Marquard opimizaion compues he Jacobian marix inernally, which may be reused, or i can be compued from scrach by numerical forward differeniaion of f (x 0 ). For small values x x 0, he linearizaion is a good approximaion of he original funcion. This approximaion can be invered by x x 0 = J + 0 (y y 0), wih J + 0 as pseudo inverse of he Jacobian, compued by singular value decomposiion. In his formulaion, he error ε p can be propagaed individually, corresponding o each elemen of y, by ε i x = J + 0 εi, wih ε i as null vecor wih only elemen i se o ε p. If independen errors are assumed, hen he individually propagaed errors are simply he square roo of he sum of squares according o he rules of error propagaion. This is effecively he same as muliplying he pixel error wih he L 2 norm over he rows of he inverse Jacobian, i.e. ε xk = ε p J +, (11) wih ε xk as k-h elemen of he error vecor ε x and J + 0k as he k-h row of J + 0. I is imporan o undersand ha he esimaion of he visual odomery error ε x implicily includes all sources of errors due o bad condiioned scenes wih weak exure or low conras, like low number of correspondences, feaure poins ha are clusered in an image area, ec. Therefore, i is a very good value for judging he qualiy of visual odomery for fusion wih oher ego-moion sensors. To ge an esimae of he absolue moion error, all relaive error esimaes ε x have o be propagaed. The moion x = ( n ) from one image o he nex can be wrien as roaion marix R (n), ha is compued from he angle-axis noaion n, and a ranslaion vecor, such ha a poin P j+1 in he (j + 1)-h camera coordinae sysem is ransformed ino he previous camera coordinae sysem by P j = R (n) P j+1 +. The absolue moion (i.e. relaive o he firs camera coordinae sysem) is hen compued as 0k n j+1 = R 1 (R (n j ) R (n)), j+1 = R (n j ) + j, (12) wih R 1 as he funcion ha compues he angle-axis parameers from he given roaion marix. To obain he absolue moion error ε xj+1 corresponding o he (j+1)-h camera posiion, he absolue moion error ε xj is combined wih he relaive moion error ε x by adding each of he six elemens of boh error vecors individually o he corresponding elemens of x j and x. Using (12), his resuls in 12 (erroneous) esimaes x j+1 of he absolue moion from which he unbiased absolue moion x j+1 is subraced. The parameer difference beween roaions n and n in angle-axis form is compued as { n n n T n 0, n = ( ) min n n, n n 2π n n oherwise, (13) 10

11 for considering he disconinuiy of he angle-axis noaion. I is imporan o noe ha he parameer difference of he roaion is no equal o he difference roaion. I is only used for specifying he error in he corresponding parameers. According o he rules of error propagaion, he 12 individual differences/error vecors are combined by componen wise compuing he square roo of he sum of he 12 squared elemens. This leads o he error vecor ε xj+1, ha corresponds o x j+1. In he whole discussion, covariances were ignored by always assuming independen errors for reasons of simpliciy. The absolue moion error esimae is used for anoher exension of he original work, which opionally compues he rigid moion no only o he previous image, bu independenly o all images of a se of previous images. The moion esimae wih he minimum absolue moion error esimae is aken as resul. For saving compuaion ime, he se conains only a limied, small number of previous images. Afer compuing he moion, he curren image replaces eiher an image o which he moion could no be calculaed (i.e. which is oo old) or he one wih he highes overall moion error esimae. Wih his sraegy, soluions wih lower overall moion error esimaes are preferred and moion drif is minimized. The compuaion ime is minimized by soring he 3D locaions and rank signaures of he images as inermediae resuls, insead of he images hemselves. Thus, he mos expensive seps only need o be done once. We found ha his sraegy makes visual odomery very robus and reduces drif, especially in siuaions wih slow moion in comparison o he frame rae, e.g. he drif will be zero, if he sysem does no move. However, we do no uilize his opion in he curren work, because i would make moion fusion much more complicaed as he visual odomery is compued o differen images in he pas. Insead, we only used incremenal visual odomery, which calculaes moion always o he previous image. 6 Pose Esimaion In his secion, he mulisensor daa fusion algorihm as already presened in [7] is explained which uses an indirec informaion filer for fusing inerial measuremens of he IMU wih relaive ranslaion and roaion measuremens from he 3D visual odomery and 3D leg odomery of he Crawler. Sensor daa fusion is used o achieve more accurae and robus pose esimaes han obained by using moion measuremens of a single sensor. 6.1 Moion Sensors In addiion o he sereo camera running visual odomery as described in he previous secion, he DLR Crawler uses an XSens IMU and leg odomery as moion sensors. The XSens IMU is based on MEMS inerial sensors and consiss of hree acceleromeers and hree gyroscopes measuring he acceleraions in x, y, z direcion and he angular velociies around he hree axis, respecively. 3D magneomeer daa is also available bu in he curren work only calibraed acceleromeer and gyroscope measuremens are used a a rae of 120 Hz. The full scale acceleraions and raes of urn are 50 m/s 2 and 1200 deg/s, respecively. Acceleromeer noise is m/s 2 and gyroscope noise is rad/s. Since he accelerome- 11

12 ers also sense he graviy, absolue roll and pich angles can be derived. By inegraing he acceleraions and angular velociies, he velociy, posiion and orienaion of he IMU can be compued. However, since he acceleromeer and gyroscope measuremens are biased, he errors in posiion and orienaion will grow unbounded due o he inegraion. For his reason, he IMU needs o be correced by oher sensors wih less drif o give good posiion esimaes. The advanage of he IMU is ha i only depends on he presen graviy and apar from ha is independen of environmenal condiions. Using he leg join angle and join orque measuremens, a 6 DOF odomery of he DLR Crawler is compued. I esimaes relaive pose changes of he robo based on maching poin clouds, which are represened by he posiions of he supporing fee. The algorihm assumes rigidiy of he configuraions, which implies a no slip condiion for he fee. Hence, he qualiy of he relaive leg odomery measuremens depends on he ground condiions. Since he basic odomery is subjec o srong drif of he pich and roll angles, he join orque sensors are used o compue an esimae of he earh graviy direcion which allows o sabilize he absolue roll and pich angles using an error sae Kalman filer. For opimally combining he measuremens of all available moion sensors, a mulisensor daa fusion filer was developed. Since he IMU sends daa a he highes rae and is independen of environmenal condiions, i was chosen o be he main sensor for pose esimaion. The IMU is aided by relaive visual odomery and relaive leg odomery measuremens. Leg odomery and visual odomery can be considered as complemenary because usually rough errain, where leg odomery is prone o slip, has good exure and allows accurae visual odomery measuremens, and vice versa. 6.2 Filer Choice For Mulisensor Daa Fusion For mulisensor daa fusion, usually, probabilisic esimaors such as he Kalman filer or is inverse formulaion, he informaion filer, are used. In his applicaion, an indirec feedback informaion filer is used. The informaion filer has he advanage ha fusing measuremens of muliple sensors a he same ime can be achieved very easily. The indirec or error sae form works on an error sae vecor which conains he errors of he acual sae raher han he sae variables hemselves. The advanage is ha no model of he usually nonlinear robo dynamics is required bu he filer is based on linear equaions describing he error propagaion in he inerial sysem. The feedback formulaion means ha he esimaed error is fed back ino he IMU navigaion equaions o correc he curren posiion, velociy and orienaion esimaes. For his, he esimaed error saes are kep small and small angle approximaions in he filer equaions are possible. Tha also means ha he error sae can be prediced as zero for each new filer sep. Furhermore, he indirec filer formulaion allows he filer o be run a a lower frequency han he inerial navigaion equaions. For a more deailed discussion of he differen filer formulaions he reader is referred o Roumeliois e al. [34]. The informaion filer is numerically equivalen o he Kalman filer bu has inverse complexiy properies. In paricular, while he predicion sep of he Kalman filer is compuaionally simple and he updae sep is complex, he informaion filer equaions yield a complex predicion sep and a compuaion- 12

13 ally cheap updae sep. For ransforming he indirec Kalman filer ino he informaion form, he informaion marix Y and he error informaion vecor y are defined as Y = P 1 and y = Y x, (14) where P is he esimaion covariance marix and x is he error sae vecor. Transforming he Kalman filer equaions such ha Y and y are esimaed resuls in he predicion sep Y y = (A Y 1 1 AT + Q p ) 1 (15) = Y (A Y 1 1 y 1), (16) where A is he sae ransiion marix and Q p is he process noise marix. In he feedback form, he predicion (16) can be simplified o y = 0 because i is assumed ha he error is correced afer each filer sep. The updae sep of he informaion filer becomes Y = H T (Q m ) 1 H + Y (17) y = H T (Q m ) 1 z + y, (18) where H is he measuremen marix and Q m is he measuremen noise marix. In he indirec formulaion, he measuremen vecor z is he difference beween he IMU measuremens and he measuremens of an aiding sensor. The updae sep can be wrien as Y = I + Y, wih I = H T (Q m ) 1 H, (19) y = i + y, wih i = H T (Q m ) 1 z. (20) The erm I is he amoun of informaion in he measuremen and i is he conribuion of he measuremen z o he sae vecor [9]. If here are several measuremens z k, a a imesep we ge I = i = n H T k,(q m k,) 1 H k, = k=1 n H T k,(q m k,) 1 z k, = k=1 n I k, (21) k=1 n i k,. (22) The simpliciy of he updae sage of he informaion filer originaes from he fac, ha he measuremens of he single sensors are condiionally independen. Hence, he informaion form of he Kalman filer has compuaional advanages for mulisensor daa fusion. The rouines for compuing I k, and i k, for each measuremen are independen of each oher and independen of Y and y and can run in parallel and on disribued sysems. The disadvanage is, ha a marix inversion is required o obain he error sae vecor x from he informaion vecor y. However, he more exernal sensors are used, he higher he benefi of using he informaion filer. k=1 13

14 6.3 Sae Vecor and Sae Transiion Model For implemening he informaion filer we chose o use a sae vecor consising of 15 variables: The posiion p (3), he velociy v (3), he orienaion Euler angles ϕ (3), he bias of he gyroscopes b g (3) and he bias of he acceleromeers b a (3). In he indirec formulaion he error sae vecor x = ( p, v, ϕ, b g, b a ) T (23) is used. The posiion p and velociy v variables are given in world coordinaes wih he origin locaed a he IMU origin a he beginning of he daa fusion process. The Euler angles ϕ are he angles of he roaion marix ha urns a poin from he IMU coordinae sysem o he world coordinae sysem. The bias values b g and b a are given in IMU coordinaes. The use of Euler angles for represening he orienaion of he robo is valid in his applicaion, because configuraions which cause he Euler angle gimbal lock problem (such as 90 pich) will no be reached by he robo. Euler angles have been chosen because hey provide an inuiive represenaion of orienaion. For applicaions where gimbal lock can occur represenaions such as roaion vecor or quaernions should be used. However, in he error sae vecor, he orienaion error ϕ always conains small Euler angles, which are, hus, equivalen o he componens of a roaion vecor. This can easily be shown by applying small angle approximaion when compuing a roaion marix from Euler angles and from a roaion vecor. The discree ime error sae propagaion originaes from he inerial error dynamics [37] as x = A x 1 (24) 0 I R (a b a,) 0 R A = I R (25) o z o y o = o z 0 o x, (26) o y o x 0 where I is he ideniy marix (no o confuse wih he informaion amoun I ), a = (a x, a y, a z ) T is he acceleraion measured by he IMU, b a, is he prediced acceleromeer bias, R is he propagaed roaion from he IMU coordinae sysem ino he world coordinae sysem and is he ime difference beween 1 and. 6.4 The Mulisensor Daa Fusion Process An overview of one ime sep of he daa fusion process is given in Fig. 4. Firs, he acceleraions a and angular velociies ω measured by he IMU are fed ino a srapdown algorihm. Considering he sae vecor x 1 from he previous filer sep, his algorihm inegraes he IMU measuremens o velociy v, posiion p and orienaion Euler angles ϕ. These values are he prediced 14

15 x b b g, a, Sae Vecor p x 1 Prediced Sae Vecor v b b p 1 v 1 1 bg, 1 ba, 1 g, 1 a, 1 Aiding Sensors IMU a, Srapdown p, v Measuremens, x Indirec Informaion Filer Esimaed Error Sae Vecor p v bg, ba, Figure 4: Overview of he mulisensor daa fusion process - + Correced Sae Vecor p v x bg, ba, sae variables. The bias values b a, and b g, are prediced o be equal o he bias values of he las filer sep. Every ime one or more measuremens of he aiding sensors are available, he indirec informaion filer is run and gives an esimaed error sae vecor x. This error sae vecor is hen subraced from he prediced sae vecor x o feedback he error. The resul is he correced sae vecor x. If no measuremens of he aiding sensors are available, he error sae vecor is zero and he correced sae will be he prediced sae. The srapdown block and he informaion filer block are described in more deail in he following secions The Srapdown Algorihm The acceleraions and angular velociies of he IMU are measured in he IMU coordinae sysem. Since he IMU moves, he acceleraions have o be ransformed ino he world coordinae sysem before inegraing hem. For his, he roaion marix R, which urns a vecor from he IMU coordinae sysem ino he world coordinae frame, has o be compued. The roaion marix can be propagaed using he gyroscope measuremens ω. Assuming a high sampling rae ( is small), he propagaion of he roaion marix can be performed as follows [3]: R R = R 1 R, (27) R, = I + sin φ φ φ = φ + 1 cos φ φ 2 φ 2 (28) φ 2 x, + φ 2 y, + φ 2 z, (29) φ = (ω b g,). (30) is he propagaed roaion marix, which is compued from he roaion marix R 1 of he las ime sep and a differenial roaion R,. The variable φ is he roaion vecor. 15

16 Knowing he roaion marix, he IMU velociy v and posiion p can be compued. The acceleraion measuremens a have o be compensaed for bias b a,, ransformed ino he world frame using R and he graviy vecor g = (0, 0, ) T mus be compensaed: v = v 1 + (R (a b a,) + g) (31) p = p 1 + v (R (a b a,) + g) 2. (32) The Indirec Informaion Filer Wihin he indirec informaion filer relaive and absolue measuremens are used o compue he esimaed error sae vecor. While absolue measuremens only depend on he curren sae of he sysem, relaive measuremens conain a difference beween he curren sysem sae and a previous sae. Since Kalman filer heory assumes ha a measuremen only depends on he curren sae of he sysem, relaive measuremens have o be reaed in a special way. When his fac is ignored, he daa fusion filer migh also give good resuls a he firs glance. However, viewed more closely, he resuling esimaed variances are no feasible: For example, when fusing an IMU wih only relaive posiion measuremens, one would expec ha he esimaed posiion variance grows wih ime, because summing up relaive posiion measuremens resuls in a drifing posiion esimae. However, if he relaive characer of he posiion measuremens is ignored, he resuling posiion variances are esimaed small and consan over ime. This can cause serious problems when using anoher posiion sensor such as GPS, which gives absolue bu noisy posiion measuremens. These measuremens will no influence he esimaed posiion o he expeced exen because he posiion esimae afer fusing only he relaive measuremens is overconfiden. Even if no absolue measuremen was available for a longer ime and posiion drif is significan, he esimaed posiion variance would be small. To avoid his problem, he sae vecor and covariance marix have o be augmened o also conain he previous sae which is par of he relaive measuremen. This approach was described by Roumeliois e al. [33] and ermed Sochasic Cloning. This mehod inroduces he correlaions beween he curren and he previous sae and hence allows o esimae a correc covariance marix, wih growing variances over ime if only relaive measuremens are available. To keep he augmened covariance marix small, we chose o only clone he covariances associaed o he saes p and ϕ, because only relaive posiion and roaion measuremens are used. A each ime = Sar when a leas one relaive measuremen sars, he covariance marix is augmened as follows: [ ] p ˇx = ˇP ϕ = Cov(ˇx, ˇx ) (33) P aug = [ ˇP Sar Cov(ˇx Sar, x ) Cov(x, ˇx Sar ) P ], (34) where Cov(x, ˇx Sar ) is he covariance beween he saes a ime and he cloned saes a Sar. Since he covariance ˇP Sar mus no change during predicion of he filer, he sysem marix A aug and he process noise marix 16

17 Q p,aug become A aug = blkdiag [I, A ] (35) Q p,aug = blkdiag [0, Q p ], (36) where blkdiag [U, V ] sands for a block diagonal marix wih he marices U, V on is main diagonal. Since in he informaion filer he inverse covariance is used, i mus be ensured ha in he predicion sep (15) A aug P aug (A aug ) T + Q p,aug is inverible. For ha reason, if wo differen relaive measuremens sar a he same ime, cloning is applied only once o keep he covariance marix full rank. If measuremens sar a differen imes, he covariances beween he differen previous saes also have o be cloned correcly. A he end of each relaive measuremen, he corresponding covariances are deleed from he augmened covariance marix because hey are no needed any longer afer he relaive measuremen was processed. However, in his applicaion, usually a relaive measuremen sars a he same ime he previous measuremen ends. Thus, afer deleing a previous sae, he curren sae is cloned for augmening he covariance marix again. In his applicaion, relaive measuremens from visual odomery and leg odomery are usually aken a differen raes. Thus, he augmened sae vecor usually conains wo differen previous posiions and orienaions, each corresponding o he saring ime of a relaive measuremen. Mulisensor Indirec Informaion Filer Srapdown IMU Visual Odomery Leg Odomery R T T p 0.. a 0.. rel, VO rel, VO R rel, LO rel, LO Predicion Y aug y aug ( A 0 IF Absolue Roll, Pich aug aug1 Y 1 IF Relaive Ro., Transl. IF Relaive Ro., Transl. i aug Euler, I i aug Euler, aug VO, I i aug VO, aug LO, I A aug LO, aug T aug Updae + i aug Y aug I aug y + + Q p, aug ) x 1 aug Y Y aug y aug I aug aug aug aug y i aug1 aug Y y Esimaed Error Sae Vecor x p v bg, ba, Figure 5: Overview of he mulisensor daa fusion informaion filer The daa flow wihin he indirec informaion filer for mulisensor daa fusion is shown in Fig. 5. Firs, he augmened informaion marix is prediced. Then, for each available sensor measuremen k a ime sep he values for i aug k, and I aug k, are compued using he differences beween he srapdown algorihm resuls and he sensor measuremens. Then, all available informaion amouns I aug k, and informaion conribuions i aug k, are summed up and he updae equaions are performed. In he end, he resuling informaion vecor is ransformed ino an error sae vecor from which he cloned saes are deleed. These seps are described in more deail in he following secions. 17

18 Predicion Using he sae ransiion marix A aug as given in (25) and (35), he informaion marix Y aug is prediced using (16). The predicion of he informaion vecor simply becomes y aug = 0 because in he indirec feedback informaion filer he error is correced afer each filer sep. Absolue Roll and Pich Angle Measuremens Since he acceleromeers of he IMU sense he graviy, which is known in size and direcion wih respec o he world frame, i is possible o deermine he absolue roll and pich angles γ abs and β abs of he acceleraion measuremen a = [a x, a y, a z ] T as follows: γ abs = aan2(a y, a z ), (37) β abs = aan2( a x, a y sin γ abs + a z cos γ abs ). (38) From he absolue roll and pich angles, an absolue roaion marix R abs can be compued using cβcα sγsβcα cγsα cγsβcα + sγsα R abs = cβsα sγsβsα + cγcα cγsβsα sγcα sβ sγcβ cγcβ sϕ = sin ϕ abs cϕ = cos ϕ abs (39) For his, he yaw angle α abs is se o be equal o he yaw angle of he propagaed roaion marix R because i canno be deermined from he acceleraion measuremens. The absolue roll and pich angles obained from he acceleraion measuremens conain a high level of noise. The noise is caused by addiional acceleraions ha occur when he robo moves. Hence, he absolue noisy angles mus be fused wih low-noise angular measuremens. The propagaed roaion marix R as compued in (27)-(30) conains he roll and pich angles from inegraing he gyroscope measuremens. These angles do no suffer from high noise bu from a drif caused by inegraing he sensor values. By fusing R abs and R, he roll and pich Euler angles can be deermined quie accuraely wihou drif and high noise. The difference roaion marix beween he propagaed roaion R and he absolue roaion R abs is compued as Using he equaions α = aan2(r (2,1), R (1,1) ) R diff = R R T abs. (40) β = aan2( R (3,1), R (2,1) sin α + R (1,1) cos α) γ = aan2(r (1,3) sin α R (2,3) cos α, R (1,2) sin α + R (2,2) cos α) (41) o exrac Euler angles from he elemens R (i,j) of a roaion marix, he angle differences γ diff and β diff can be compued from R diff which give he measuremen vecor z Euler, : [ ] γdiff z Euler, = (42) β diff 18

19 The measuremen marix H Euler, which projecs he sae vecor x ono he measuremen vecor z Euler, is H Euler, = [ ] I (43) For he augmened sae vecor, he measuremen marix has o be augmened wih zeros o H aug Euler, = [ 0 H Euler, ], (44) because he measuremen does no depend on any previous saes bu is absolue. The measuremen noise marix Q m Euler, conains he variances of he absolue roll and pich angle measuremens and can be found by filer uning. Knowing z Euler,, H aug Euler,, Qm Euler, he informaion conribuion i aug Euler, and he informaion amoun I aug Euler, are compued using (19)-(20). Using absolue angles obained by acceleromeer daa as measuremens for he daa fusion filer violaes Kalman filer heory which assumes ha measuremen noise and process noise are uncorrelaed. Hence, he filer resul is subopimal. However, he subopimal filer resul is sill beer han no using absolue roll and pich angle measuremens for limiing he drif of he orienaion esimaes. Relaive Translaion and Roaion Measuremens The relaive moion measuremens have o be fused wih he relaive roaions and ranslaions compued by he srapdown algorihm wihin he same ime period. Visual odomery as well as leg odomery provide relaive posiion and orienaion measuremens beween wo consecuive images or robo poses, respecively. Visual odomery and leg odomery are fused in he same way as relaive ranslaion and roaion measuremens. Thus, hey will boh be referred o as odomery sensor and no be disinguished in he nex paragraphs. A relaive measuremen has wo imesamps sar and end a he beginning and he end of he relaive measuremen. Furhermore, for fusing relaive roaions and ranslaions, all values mus be represened in he same coordinae sysem. Tha means, he relaive measuremens of all sensors have o be ransformed ino relaive measuremens in he IMU coordinae frame in order o be fused wih IMU measuremens. Tha also means, ha he ransformaions beween he differen sensor coordinae frames mus be known, eiher by design or by calibraion. The differences beween he relaive moion given by he srapdown algorihm in he ime inerval from sar o end and he relaive moion measured by he odomery sensor give he measuremen vecor z rel,. In order o compue he difference beween wo relaive roaions R I rel measured by he IMU and R S rel measured by an odomery sensor, an absolue roaion marix has o be compued. To preserve he relaive characer of he measuremens, boh relaive roaions have o be muliplied wih he same absolue roaion marix R sar o ge pseudo-absolue roaion measuremens. This absolue roaion marix R sar should be he bes esimae of he roaion from he IMU ino he world frame a ime sep sar : R I end = R sar R I rel, R S end = R sar R S rel. (45) 19

20 Now he roaional difference marix can be compued as R diff = R I end (R S end ) T. (46) The measuremen vecor z rel, conains he differences p diff beween he wo relaive ranslaions and he angle differences ϕ diff compued from R diff using (41): z rel, = [p diff, ϕ diff ] T. (47) The augmened measuremen marix H aug rel, which projecs he augmened sae vecor x aug ono he measuremen vecor z rel, is H aug rel, = [ ] H rel,sar H rel,. (48) [ ] I3 3 0 H rel, = (49) I In he measuremen marix H aug rel, he relaive characer of he measuremens mus be represened. This is achieved by he marix H rel,sar which conains an ideniy marix in he columns corresponding o he locaion of he cloned covariance of ime Sar and zeros everywhere else. The measuremen noise marix Q m rel, is compued from he sandard deviaions of he relaive posiion and roaion measuremens. For leg odomery, he measuremens errors depend on how much he fee of he robo slip on he ground. On homogeneous ground he amoun of slippage can be assumed consan and found by filer uning. Using visual odomery, assuming consan sandard deviaions for he relaive moion measuremens is no appropriae for environmens wih changing ligh or exure condiions. Thus, he esimaed errors of each visual odomery measuremen (ref. Sec. 5.3) are ransformed ino he IMU coordinae sysem using error propagaion and hen fed ino he measuremen noise marix. Knowing z rel,, H aug rel,, Qm rel, he informaion conribuion i aug rel, and he informaion amoun I aug rel, are compued using (19)-(20). Updae A every ime sep, i aug k, and I aug k, of each available sensor measuremen are compued. In he final sep of he mulisensor informaion filer, hese values are summed and used o updae he prediced informaion vecor and informaion marix using (19)-(20). Finally, he resuling informaion vecor y aug is ransformed ino an error sae vecor x aug. From x aug, x is exraced, which conains he esimaed errors of he single robo saes. By invering he resuling informaion marix Y aug, he covariance marix P aug can compued if required and P can be exraced Error Sae Feedback To correc he posiion, velociy and bias values of he prediced sae vecor x, he corresponding error esimaes from he error sae vecor x are subraced. For feeding back he esimaed roaion angle error ϕ, a roaion marix R corr has o be compued from ϕ using R corr = I + ϕ (50) 20

21 and correcion is performed as R = R T corr R. (51) From R he correced Euler angles can be exraced via (41). 6.5 Filer Iniializaion In he beginning of he daa fusion process he robo is moionless in is saring posiion. This phase can be used for filer iniializaion. From he very firs IMU measuremen, he saring orienaion R 0 wih respec o he graviy vecor is esimaed from he acceleraion measuremens a 0 as shown in (37)-(38). Furhermore, he bias esimaes b a,0 and b g,0 are iniialized using he saring orienaion, he known graviy vecor g and he gyroscope measuremens ω 0. Good saring values for he sensor biases are b a,0 = a 0 + R T 0 g (52) b g,0 = ω 0. (53) From he following IMU measuremens, he esimaes of he bias values and he saring orienaion can be refined exploiing he fac ha he robo does no move. Hence, posiion, velociy and orienaion measuremens wih he value of zero and small noise marices are fed ino he informaion filer. As a resul he bias value esimaion sabilizes. Furhermore, he absolue roll and pich angle measuremens from he acceleraions are fused wih he orienaion measuremens from he gyroscopes as described in secion The iniializaion phase is finished when he change in he bias esimaes drops below a hreshold. This process usually akes a few seconds. Once he informaion filer is iniialized, he robo can sar moving and visual odomery and leg odomery measuremens are used. 6.6 Filer Resuls Resuls of using he mulisensor daa fusion filer for posiion esimaion compared o using only visual or leg odomery are shown in Fig. 6. In his experimen, he Crawler was seered manually along a recangular pah in a esbed filled wih gravel. An exernal infrared racking sysem provided ground ruh rajecories. Fig. 6(a) shows he es seup wih he robo in is saring pose and he approximae seered pah. Fig. 6(b) shows he ground ruh rajecory measured by he racking sysem, he fusion resul and he differen odomery rajecories which were obained by summing he relaive measuremens of he respecive sensors. The rajecory compued using only he IMU measuremens is no shown here because is enormous drif leads o an error of more han 100 m afer 60 s runime. The visual odomery rajecory is quie accurae apar from a small drif of he yaw angle. The leg odomery rajecory shows ha yaw angles are overesimaed because of slip in he gravel (ref. Fig. 6(d)). The fusion rajecory is very close o he ground ruh pah. Fig. 6(c) shows plos of he z-coordinaes. While visual odomery and leg odomery drif due o roll and pich angle errors, he esimaed z-coordinae of he fusion resul remains close o he ground ruh curve because of absolue roll and pich angle measuremens from he acceleromeers. Fig. 6(e) shows he sandard deviaions of he posiion 21

22 esimaes compued from he esimaion covariance marix. As can be seen, he sandard deviaions grow wih ime since no absolue posiion measuremens are available. The deailed plo in his figure illusraes he influence of he relaive measuremens on he covariance: Visual odomery measuremens usually have lower uncerainy han leg odomery measuremens and, hus, reduce he esimaion covariance more han he leg odomery measuremens. However, during urning in he corners of he esbed, he errors of he visual odomery measuremens are higher. The reason for ha is he exure of he esbed walls which is worse han he exure of he gravel. Hence, he covariances increase sronger during hese periods. A more deailed overview of he filer performance including experimenal resuls under poor visual condiions are given in [7]. 7 Mapping A digial errain model (DTM), which is incremenally buil from he deph images compued by SGM, was chosen as inernal map. The DTM represens he environmen as a regular grid and each grid cell sores a single heigh value. Alhough his model canno be used o represen muliple heigh values per grid cell, i is sufficien for many applicaions where overhangs are rare, such as planeary surface exploraion. DTMs need only lile sorage space in comparison o full 3D models and pah planning algorihms can be applied easily. Mapping is performed in wo seps. Firs, a local DTM is creaed from a single deph image. Afer ha, he local DTM is aached o he global DTM. The radiional mehod for creaing a local DTM from a range measuremen such as a deph image is he reconsrucion of he 3D coordinaes P c = (P c x, P c y, P c z ) in he camera coordinae frame c from he image poins p i lx, pi ly, pi d in he image coordinae frame i using P c x = pi lx p i d P c y = pi ly p i d P c z = f p i d = pi lx p i lx pi rx = (pi ly + pi ry) 2 (p i lx pi rx) = f p i lx pi rx (54) (55) (56) wih f as focal lengh in pixels and as sereo baseline. The resuling poin cloud is hen projeced ono he grid. This approach was presened in [6]. Alhough he sereo dispariy image is dense, his approach can resul in sparse maps because he opical axis of he camera is usually no perpendicular o he surface bu he surface is viewed a a cerain angle. The locus mehod [26] is an efficien way o creae dense heigh maps wih arbirary resoluion from range images. I finds he elevaion z a a poin (u x, u y ) of a reference plane by compuing he inersecion of he errain wih a verical line a (u x, u y ). The projecion of he verical line on he image is called locus. The inersecion poin wih he errain is compued in image space raher han Caresian space. For each cell in he DTM a vecor represening a 22

23 (a) Tes seup and seered rajecory y [m] Tracking Leg Odomery Visual Odomery Fusion Sar x [m] (b) Recorded rajecories 0.1 Posiion z [m] Tracking 0.05 Leg Odomery 0.1 Visual Odomery Fusion ime [s] (c) z-coordinaes Yaw angle [degrees] Tracking Leg Odomery Visual Odomery Fusion (d) Yaw angles ime [s] (e) Sandard deviaions compued from he esimaion covariance marix Figure 6: Tes run for filer performance evaluaion verical line l is compued as l = (u, v) = ( [u x, u y, u z ] T, [v x, v y, v z ] T ), (57) where u is a poin and v is a uni direcional vecor. Each poin r on he line 23

24 is hen represened by r = u + λv. (58) For a verical line in world coordinaes he values of u and v become u = [u x, u y, 0] T (59) v = [0, 0, 1] T. (60) Using he roaion marix R w c which urns a poin from he camera coordinae frame c o he world coordinae frame w and he ranslaion w from he world coordinae frame o he camera coordinae frame, his line can be ransformed ino camera coordinaes: l c = (u, v) = ( (R w c ) T (u w ), (R w c ) T v ). (61) By projecing l c ono he dispariy image, a generalized locus is obained. The inersecion of he locus wih he dispariy profile gives he elevaion of he grid cell a (u x, u y ). The projecions r i = (rlx i, ri ly, ri d )T of all poins r c of line l c ono he image also form a line l i wih r i d = f r c z (62) r i lx = ri d rc x = f rc x r c z (63) r i ly = ri d rc y = f rc y rz c. (64) The inersecion of l i wih he dispariy profile of he image has o be compued. Due o he orienaion of he camera wih respec o he world coordinae sysem, i is beneficial o parameerize he locus line by he image row coordinae rly i, because a verical line in world coordinaes will mos likely run hrough all rows in he image, bu will only appear in few columns. Hence, line equaions for he column rlx i = f(ri ly ) = a xrly i + n x and dispariy rd i = f(ri ly ) = a drly i + n d can be creaed by ransforming wo disan poins r c 1 and r c 2 on l c ino image coordinaes (rlx1 i, ri ly1, ri d1 ) and (ri lx2, ri ly2, ri d2 ) and deermining he line equaion parameers slope a and offse n for dispariy (index d) and column (index x). a x = ri lx1 ri lx2 r i ly1 ri ly2 a d = ri d1 ri d2 r i ly1 ri ly2 n x = r i lx1 a x r i ly1 (65) n d = r i d1 a d r i ly1 (66) The firs sep for finding he inersecion is o search wo sample poins (rlx1 i, ri ly1, ri d1 ) and (ri lx2, ri ly2, ri d2 ) on he line which fulfil he condiion r i d1 < p i d(r i lx1, r i ly1) (67) r i d2 > p i d(r i lx2, r i ly2). (68) For doing so, he firs sample poin is searched for by saring a he boom row of he image; searching for he second sample poin sars a he op row. 24

25 For each row, he dispariy and column values can be compued using he line equaions above. Afer finding hose wo sample poins, binary search can be applied beween hese poins o find he inersecion rilx i, ri ily, ri id wih ri id = pi d (ri ilx, ri ily ). I should be noed ha here are image pixels which do no have a valid dispariy value. If he search reaches such a pixel, he search erminaes and a linear search is performed beween he curren inerval boundaries. If he search inerval canno be reduced any furher and he dispariy difference beween he boundary values p i d (ri lx1, ri ly1 ) and pi d (ri lx2, ri ly2 ) is wihin a cerain hreshold (range shadows), he inersecion is found and calculaed as r i ilx = ri lx1 + ri lx2 2 r i ily = ri ly1 + ri ly2 2 (69) (70) r i id = ri d1 + ri d2 2 (71) If muliple inersecion poins are found by he linear search, only he inersecion wih he highes elevaion is considered. From he inersecion poin, he elevaion value can be calculaed using he reconsrucion formulas in (54)-(56). Fig. 7 shows he op views on local maps creaed by he radiional approach and by he locus mehod wih colors indicaing he elevaions. As can be seen, he locus mehod gives denser errain maps wih less arifacs. (a) Tradiional approach (b) Locus mehod Figure 7: Comparison of a local map creaed by he radiional and he locus mehod Since in sereo vision he error of reconsrucing he disance of an objec poin grows quadraically wih he disance of ha poin from he camera, he size of he local map was limied o 1 m. Tha prevens erroneous range measuremens from being insered ino he errain map. Each local DTM from a deph image is aached o he global DTM using he esimaed robo pose a he ime of image acquisiion. Exising heigh values are overwrien by newer values. This approach is prone o errors from pose esimaion, which can cause arifacs in he DTM. However, hese errors remain 25

26 small for small scale maps and can be considered in he raversabiliy esimaion process by aking he ime ino accoun when he heigh value was insered ino he global map. 8 Traversabiliy Esimaion Based on he global errain model, he raversabiliy of he cells is assessed as presened in [6]. Each cell is assigned a danger value d (d {[0, 1], }) describing he errain difficuly. A cell is raversable if he robo is no exposed o criical errain hazards irrespecive of is orienaion given is cener is locaed in ha cell. Thus, he robo can be reaed as a poin in furher compuaions. A danger value of d = 0 sands for compleely fla, smooh errain, which can be raversed by he robo mos easily. Higher danger values are assigned o areas which are harder o pass. A value of d = 1 describes errain which is jus barely raversable for he robo. Unraversable regions are assigned d =. Unknown areas are assumed o be raversable bu are assigned a high danger value of d = 1. There are hree poenial hazards: seep slopes, high errain roughness and high seps. Each of hese crieria mus be esimaed from he DTM. If one of he hazard crieria exceeds he corresponding criical value, he cell is marked as unraversable. The criical values s cri, r cri and h cri are he maximum slope, roughness and sep heigh which he robo can raverse wihou ipping over or geing suck. For raversable cells he danger value is compued from he hree ypes of hazards as s r h d = α 1 + α 2 + α 3, (72) s cri r cri h cri where α 1, α 2 and α 3 are weigh parameers which sum up o 1. Similar o he raversabiliy esimaion in GESTALT [12], he slope s of a cell is calculaed by fiing a plane in a circular region around he cell wih a diameer corresponding o he maximum diameer of he robo. The angle beween he plane normal and he z-axis of he global coordinae frame gives he slope inclinaion s. The errain roughness r is calculaed as he sandard deviaion of he errain heigh values from he compued plane in he circular region around he cell. The sep heigh h is compued in wo seps. Firs, local heigh differences wihin a square window of several (e.g ) grid cells are compued for all cells in he circular region. If he maximum heigh difference beween any cell in ha window and he cener cell of he window is greaer han he criical sep heigh h cri and he slope beween he corresponding wo errain poins is higher han he criical slope s cri, he maximum heigh difference is sored as he emporary sep heigh of he cenral cell of he window. Second, he sep heigh of he cenral cell of he circular region is compued as h = min(h max, h max n s n cri ), (73) where h max is he maximum emporary sep heigh in he circular region, n s is he number of cells in he circular region whose emporary sep heighs are higher han he criical sep heigh and n cri is he valid number of cells (e.g. 50) 26

27 (a) DTM (c) Slope (b) Danger Value (d) Roughness (e) Sep Heigh Figure 8: Danger value compuaion from he crieria slope, roughness and sep heigh (scri = 20, rcri = 30 mm, hcri = 50 mm, α1 = 0.5, α2 = 0.25, α3 = 0.25) wih a emporary sep heigh higher han he criical sep heigh. This mehod for calculaing he sep heigh also deecs small seep slopes as seps and is robus o missing errain informaion. Fig. 8 illusraes he compuaion of he danger value from he hree crieria. I shows ha he sep heigh is well suied for deecing wheher a cell is raversable or no, bu i provides lile informaion abou he difficuly of he raversable cells. By conras, he slope and roughness crieria can fail o deec unraversable cells bu are beer suied for esimaing he difficuly of raversable cells. The errain raversabiliy is reesimaed every ime a new local map is added o he global map. I is no necessary o recompue danger values for he complee global map. Only he cells wihin he area of he new local map surrounded by a border of he size of he robo have o be reesimaed. As menioned above, arifacs can be presen in he DTM, which mus no be deeced as errain hazards (ref. Fig. 9). For his reason, only heigh values which were deeced wihin a cerain range f of frame numbers are considered in he raversabiliy esimaion process. When reesimaing he danger of a cell ha was deeced impassable in he previous esimaion sep, heigh values of a greaer frame range f 0 > f are considered in comparison o reesimaing a previously raversable cell. Pracical ess showed ha aking only heigh values wihin f ino accoun ofen causes unraversable cells o be deeced as raversable cells because nearby hazards are no covered by he images. Thus, also considering older heigh values wihin f 0 for reesimaing he raversabiliy of previously unraversable cells gives more realisic resuls. The raversabiliy of a cell is only compued if a sufficien number of heigh values is presen in he circular rover-sized region around he cell. In addiion o he danger value, a cerainy value is calculaed as he percenage of available heigh values in he circular region. This value is laer required o decide wheher he acive exploraion of an area is necessary. The described raversabiliy esimaion approach depends on many parameers such as he size of he robo, he criical sep heigh, roughness and slope values and he weigh of he hree crieria for calculaing a danger value. These 27

28 Figure 9: Traversabiliy esimaion of a DTM wih arifacs caused by errors in posiion esimaion: By considering only heigh values wih frame numbers wihin f = 20 for passable and f = 200 for impassable cells arifacs are no deeced as hazards. parameers allow o adap he raversabiliy esimaion process o differen ypes of robos. In addiion o he DLR Crawler, we successfully use his mehod for generaing raversabiliy maps for differen wheel-driven robos and for finding fla fooholds for a biped walking robo. However, while some of he parameers are known from he design of he robo, ohers have o be found empirically in pracical ess. Here, in fuure, a learning approach could be implemened so ha each robo can learn suiable parameer values by iself. 9 Pah Planning Based on he raversabiliy map, he robo can plan a pah o he goal poin. Since he robo does no have an a priori map of he environmen, is knowledge abou he errain changes over ime. The pah planner mus be able o adap he pah o changes in he map in an efficien way. Thus, a pah planner of he D* family was chosen. D* developed by Senz [36] is he dynamic version of he A* graph based pah planning algorihm. These search algorihms find he minimum cos pah o a goal verex in a graph by firs searching hose verexes which mos likely appear o lead o he goal. In conras o A* planners, D* is able o modify previous search resuls locally and, hus, is more efficien when dynamic replanning is required. For he presen navigaion sysem a D* Lie [24] pah planner was used, which is simpler and more efficien han he classic D* algorihms. To apply he D* Lie planner o he grid map, he map has o be considered as a graph. The grid cells are he verexes of he graph and edges connec verexes which correspond o adjacen cells in he grid map. As for he A* algorihm, a cos funcion and a heurisic disance funcion mus be implemened for he D* Lie pah planner. However, D* Lie plans a pah in opposie direcion from he goal verex G o he sar verex S. The cos funcion c(n, N ) describes he cos for moving from verex N o is neighbor N. The heurisic disance funcion h(n, S) is an esimae of he coss remaining o reach he sar verex from he curren verex N. The formulaion of he cos funcion defines he opimaliy of a pah. Ofen, a pah is opimal if i is he shores pah o he goal. In he presen work, no only he pah lengh bu also he raversabiliy 28

29 (a) β = 3 (b) β = 10 Figure 10: Pahs planned wih differen values of β. of he pah cells should be aken ino accoun. Thus, he cos funcion for going from verex N o is neighbor N is c(n, N ) = (N x N x) 2 + (N y N y) 2 + β d(n ). (74) The firs erm describes he disance beween he verexes and he second erm denoes he danger value of he desinaion verex weighed by β > 1. The bigger he value of β is chosen he longer pahs are acceped if hey go hrough safer cells (ref. Fig. 10). The coss of going o an unraversable cell are. The heurisic disance funcion is imporan for he efficiency of he planner. For he planner o be opimal i mus fulfill he monoony condiion h(n, S) c(n, N ) + h(n, S). (75) Tha implies ha h(n, S) mus no overesimae he rue coss h (N, S) of going from N o S along he shores pah. Since he minimum danger value of a cell is 0, he heurisic disance funcion mus be h(n, S) = (N x S x ) 2 + (N y S y ) 2, (76) which is he direc disance from N o S. Due o he riangle inequaliy, his funcion also fulfills he monoony condiion. This use of D* Lie for visual navigaion has been presened in [6]. In he beginning, he robo does no have any informaion abou is environmen. I plans an iniial pah, which is he direc pah o he goal. As he robo follows his pah, i collecs informaion abou he environmen. If assumpions abou he raversabiliy of he pah cells are proven wrong by new daa, he pah is replanned from he curren cell of he robo. From he definiion of he cos funcion follows approximaely ha pahs are planned which are β imes longer han he shores pah, if heir average danger value is less han 1 β of he danger of he shores pah. Tha means, ha only he relaion beween pah lengh and pah safey is considered bu no he absolue danger value of a pah. However, if he robo is carrying a heavy load or if is hardware is damaged, he pah planner mus adap he pah o he changed moion abiliies of he robo. To avoid reassessing he raversabiliy of he whole errain map, a danger value hreshold 0 d max 1 can be se in he pah planner. If he danger value of a cell is higher han d max, he coss of moving o ha cell are se o. Thus, he safey of he planned pah is 29

30 improved. Currenly, he danger value hreshold is se by he operaor, bu in fuure, he robo should be able o se he hreshold according o is esimaed moion capabiliies. For his, he robo will have o learn which maximum danger value corresponds o is curren moion capabiliies. Furhermore, he value for β should also be learned by he robo insead of being se o a fixed value by he operaor. The use of he danger value hreshold was shown in [13]. 10 Moion Conrol Pah following is achieved by a simple proporional conroller which sends he moion commands move forward, urn lef and urn righ as well as he maximum danger value of he upcoming pah cells o he walking layer. Thus, in easy, smooh errain he robo can use he simple and fas ripod gai, and in rough and more difficul errain he gai can be swiched o he compuaionally more expensive biologically inspired gai wih elevaor reflexes for overcoming higher obsacles. Depending on he horizonal opening angle of he sereo camera, i migh be possible ha he robo is no able o perceive enough informaion abou he upcoming errain o calculae he raversabiliy wih high cerainy. Furhermore, he pah could lead ino a region ha is currenly ouside he view of he robo bu could be perceived if he robo would urn. In hese cases, acively exploring he environmen of he pah is necessary. For his, he moion conroller can command exploraion urns as presened in [6] o he robo. An exploraion urn is necessary, if he cerainy value of a pah cell which is in range of he cameras is lower han 1. During an exploraion urn he robo urns over an angular range of 2ɛ so ha he cameras cover he rover-sized circular region around he pah cell being explored. Since a cerainy value of 1 is hard o reach in pracice, a se of rules abou when exploraion urns are permied has been esablished: Beween wo exploraion urns he disance beween he pah cell o be explored and he previously explored pah cell mus be a leas l and one of he following condiions mus hold: The robo mus have passed a disance of a leas l. The pah cells o be explored in wo subsequen exploraion urns mus be a an angle of a leas ɛ given he curren robo posiion is he pivo poin. The pah mus have been replanned. These rules are necessary o avoid ha he robo repeas exploraion urns because he cerainy value does no reach 1. When he camera is mouned on a pan uni, he exploraion urns can be performed by urning only he camera insead of urning he whole robo. For he hardware used in he pracical ess he values were chosen o be ɛ = π/8 and l = 0.2 m. The navigaion algorihm erminaes successfully when he robo reaches he specified goal poin according o is esimaed posiion. Since he robo canno 30

31 reach he goal poin exacly, a olerance area mus be defined. The size of he olerance area depends on he map resoluion and he disance he robo ravels wihin one posiion esimaion sep. In he presen work, he olerance area is a circle wih a radius of 20 mm. As soon as he esimaed posiion of he robo is inside ha olerance circle around he goal poin, a sop command is sen o he walking layer and he robo sops. 11 On-line Implemenaion Excep for deph image compuaion for which a GPU implemenaion is used, he mehods presened above have been implemened in C/C++ on a Linux 2.6 GHz compuer. The navigaion algorihm consiss of several hreads. Each sensor runs in a separae hread wriing imesamped daa ino a buffer. IMU daa is buffered a 120 Hz, leg odomery daa a 10 Hz and visual odomery measuremens a abou 6 Hz. The pose esimaor hread collecs daa from all sensors and fuses hem according o heir imesamps. The resuling pose esimae is sored in anoher buffer. The mapping hread receives a imesamped deph image from he sereo camera and rerieves he maching pose from he pose esimaor hread. From hese, he raversabiliy map is compued. The mapping hread runs a a rae of abou 1 Hz. The pah planner and moion conroller also run in separae hreads. They rerieve he curren pose esimae and calculae pahs and moion commands, respecively. The moion conroller sends moion commands o he walking layer a a rae of abou 10 Hz. 12 Experimenal Resuls Figure 11: Tes seup For evaluaing he performance of he navigaion algorihm, an indoor es environmen was creaed as shown in Fig. 11. A genle slope led ino a esbed filled wih gravel. Large sones were used as unraversable obsacles. Mos of he gravel area is easy o pass for he robo. Fig. 12 shows sample images of 31

32 Figure 12: Sample images as viewed by he wide angle camera of he Crawler he esbed as viewed by he sereo camera of he Crawler (ref. secion 5). In a smaller region, he errain difficuly was increased so ha his area could only be passed using he biologically inspired gai wih he srech and elevaor reflexes. An exernal racking sysem was used for racking a arge body mouned on he Crawler, which provided ground ruh pose measuremens in comparison o he pose esimaed by he robo. The goal coordinaes (x = 2.8 m, y = 0.3 m) were given relaive o he saring posiion of he robo. Figure 13: Traversabiliy map and rajecories. B: swich o biologically inspired gai. T: swich o ripod gai In a firs experimen, he robo should reach he goal poin auonomously wihou any exernal disurbances or limiaions in is moion capabiliies. The resuling raversabiliy map, he rajecory obained by he racking sysem and he rajecory esimaed by he robo are shown in Fig. 13. The map has a resoluion of 20 mm per grid cell. The colors indicae he raversabiliy of he cells. Red cells are unraversable, green cells are easily raversable and from green o orange he difficuly of raversing a cell increases. As can be seen, he big sones and he esbed walls were deeced o be unraversable obsacles. The mehod for esimaing he errain raversabiliy labels a cell as raversable only if he robo is safe when is cener is locaed on ha cell. Thus, a region of half of he robo diameer around each obsacle is also marked as unraversable. This allows he pah planner o neglec he size of he robo bu o only plan a pah for he cener of he robo. Furhermore, he raversabiliy of he slope and he rough errain region was esimaed o be more difficul han he fla gravel areas. Hence, he robo swiched o he biologically inspired gai for crossing hese areas and swiched back o he ripod gai as soon as i enered easier 32

33 Posiion z Angles Yaw errain. y [m] Sar Tracking Leg Odomery Visual Odomery Fusion Goal Poin (a) Recorded rajecories x [m] Tracking Leg Odomery Visual Odomery Fusion 0.15 z [m] Swich o biolog. gai Swich o ripod gai Swich o biolog. gai Swich o ripod gai ime [s] 200 (b) z-coordinaes Tracking Leg Odomery Visual Odomery Fusion yaw angles [degrees] ime [s] (c) Yaw angles Figure 14: Tes run: No limiaions in moion abiliies Fig. 14 compares he rajecories measured by visual odomery and leg odomery wih he rue rajecory given by he racking sysem and he esimaed rajecory obained by fusing all moion measuremens. The rajecory esimaed by sensor daa fusion is close o he visual odomery rajecory because he error of visual odomery was esimaed o be very low. However, boh visual odomery and leg odomery suffer from a drif in he z-coordinae as well as yaw angle errors. Due o absolue roll and pich angle measuremens, he error in he z-coordinae of he fusion rajecory is small. The yaw angle error canno be correced sufficienly since no absolue yaw angle measuremens are available. The yaw angle plo also shows he exploraions urns ha were performed o gaher more informaion abou he upcoming errain. When he robo sopped, is esimaed posiion was x = 2.80 m, y = 0.33 m and z = 0.12 m. This posiion is ouside he given olerance region of 20 mm because of several reasons. Firs, here is a small ime delay beween he acquisiion of he posiion measuremens and he compuaion of he robo pose. Second, using he ripod gai, he robo canno sop immediaely bu has o finish he curren sep firs. The rue posiion of he robo a he goal poin was x = 2.75 m, y = 0.24 m and z = 0.09 m. This gives an endpoin error of 11.2 cm or 3.7% in relaion o a pah lengh of abou 3 m. This error is mainly caused by he deviaion of he yaw angle. The second es run was used o demonsrae he robusness of he navigaion 33

34 Figure 15: Traversabiliy map afer inducing visual odomery errors algorihm agains visual disurbances. While he robo was walking hrough he es environmen, is cameras were covered several imes using a shee of paper. All oher es condiions remained equal o he previous run. Fig. 16 shows he resuling rajecories. The visual odomery rajecory (ref. Fig. 16(a)) has large errors caused by covering he cameras. The leg odomery rajecory also deviaes from he rue pah due o slip. The pah esimaed by fusing all moion measuremens is very accurae. The visual disurbances did no affec he fusion resul since visual odomery errors were esimaed o be very high during hese ime seps and, hus, hese erroneous measuremens were given a very low weigh in he daa fusion process. This can also be seen in Fig. 16(d), because he esimaed sandard deviaion of he posiion esimae increases srongly during hese periods. The resuling raversabiliy map does no show any arifacs or obvious errors (ref. Fig. 15). A he goal poin, he robo esimaed o be a x = 2.81 m, y = 0.30 m and z = 0.08 m, while is rue posiion was x = 2.78 m, y = 0.26 m and z = 0.09 m. The overall endpoin error is 4.8 cm. In anoher es run, i was simulaed ha he Crawler picked up a heavy load a one poin of is pah and had o coninue o he goal poin wih limied moion capabiliies. Fig. 17(a) shows he resuling raversabiliy map. The robo sars moving owards he goal poin as in all previous es runs. Afer passing he slope, he danger value hreshold d max was se o a low value of 0.15 o simulae ha he robo is carrying a heavy load. As a resul, he Crawler avoided he difficul area of he esbed and chose he longer bu safer pah o he goal poin. The endpoin error of he esimaed posiion a he goal poin was 7.7 cm. 13 Conclusion In his paper, a sereo vision based navigaion algorihm for a six-legged walking robo in unknown rough errain has been presened. From sereo images deph images are compued using he robus and accurae SGM mehod. A visual odomery was implemened and visual odomery errors are esimaed along wih he relaive moion measuremens. For achieving accurae and robus pose esimaes of he robo, IMU daa is fused wih visual odomery and leg odomery using an indirec informaion filer. In parallel, a dense 2.5D errain 34