INTEGRATING PHOTOGRAMMETRY AND INERTIAL SENSORS FOR ROBOTICS NAVIGATION AND MAPPING

Transcription

1 INTEGRATING PHOTOGRAMMETRY AND INERTIAL SENSORS FOR ROBOTICS NAVIGATION AND MAPPING Fadi Bayoud, Jan Skaloud, Bertrand Merminod Eole Polytehnique Fédérale de Lausanne (EPFL) Geodeti Engineering Laoratory CH-115, Lausanne Astrat: Integrating visual and inertial sensors has eome a ommon pratie in navigation due to the inrease in omputer power, in algorithms advanement and in sensor improvements. One of the prolems yet to e solved is the Simultaneous Loalisation And Mapping (SLAM). SLAM is a term used in the rootis ommunity to desrie the prolem of mapping the environment and at the same time using this map to determine (or to help in determining) the loation of the mapping devie. Classially, terrestrial rootis SLAM is approahed using LASER sanners to loate the root relative to the strutured environment and at the same time to map this environment; however, outdoors rootis SLAM is not feasile with LASER. Reently, the use of visual methods, integrated with inertial sensors, has gained an interest. The urrent solutions use a single Kalman Filter with a state vetor ontaining the map and the root oordinates, whih introdues non-linearity and ompliations to the filter, whih then needs to run at high rates (2 Hz) with simplified navigation models. In this study, SLAM is developed using the Geomatis Engineering approah. Two filters are used in parallel: the Least-Squares Adjustment (LSA) for mapping and the Kalman Filter (KF) for navigation. Coneptually, the outputs of the LSA photogrammetri resetion (position and orientation) are used as the KF external measurements. The filtered position and orientation are then employed in the LSA Photogrammetri intersetion to map the surrounding features that are used as ontrol features for the resetion in the next epoh. In this manner, the KF takes the form of a navigation only filter using omplete modelling, with a state vetor ontaining the orretions to the navigation parameters and updated at low rates (1 to 2 Hz). Results show that this method is feasile with limitation indued from the quality of the images used. Although simulation showed that a position with auray of 5-1 m for ojets at distanes of up to 1 meters is possile, in pratie this is not ahieved due to the low quality of the CCDs used. The environment of testing and/or of potential appliation is very ruial eause of the importane of feature extration s auray. In this paper, the methodology is presented, differenes with other SLAM solution are pointed out, and numerial tests are provided and disussed. 1. Introdution Rootis Simultaneous Loalisation And Mapping (SLAM) is the prolem of mapping the environment surrounding the root and at the same time using this map to determine the loation

2 of the root (Csora, 1997; Newman 1999). Navigation and mapping systems are the ore elements of SLAM, without whih an exploring root annot do its jo. The appliations of selfnavigating exploring roots are aundant, ut one of the most important is: going to and exploring plaes where no man is safe to do. A map of the surrounding environment of the root and a navigation system are essential for the root to perform manoeuvres and in turn to omplete its mission. Traditionally, terrestrial rootis SLAM is approahed using LASER sanners to loate the root relative to the strutured environment and to map this environment at the same time. LASER sanners have shown to e a very good tool where the auray of loalisation is within the entimetre level. However, outdoors rootis SLAM is not feasile with LASER alone due to the asene of stereotypi features and environment s roughness. Therefore, different tools have to e used. Reently, the use of visual methods, integrated with inertial sensors, has gained an interest. These visual methods rely on exploitation of one or more ameras (or video). Yet, no lear indiation aout the mapping methods or integration algorithms is expliitly illustrated; one an onsult the Journal of Rootis Systems (24). Jung (23) has used vision motion estimation to perform SLAM. His method relies on linking the pixel s motion rate on the images with the displaement of the root. However, this filter has to run at very high rates and in ase of vision loss, no other means to re-loate the root is availale. These partiular solutions use a single Kalman Filter with a state vetor ontaining the map and the root oordinates. This introdues high non-linearities, large state vetor as well as other ompliations to the filter, whih needs to run at high rates (2 Hz) with simplified navigation models. A lassial tool for mapping is Photogrammetry, where sequene of stereo-images are aptured and self-oriented. As for the navigation systems, the oupling of Inertial Navigation System (INS) and Gloal Navigation Satellite Systems (e.g., the GPS) is indispensale. In addition, photogrammetry an also e used for loalisation positioning. In this paper, SLAM is solved y employing two CCD ameras and one IMU. Two filters are used in parallel: the Least-Squares Adjustment (LSA) for mapping and the Kalman Filter (KF) for navigation. Coneptually, the outputs of the LSA photogrammetri resetion (position and orientation) are used as the KF external measurements. The filtered position and orientation are then employed in the LSA Photogrammetri intersetion to map the surrounding that is used (as a set of ontrol features) for the resetion in the next epoh. In this manner, the KF takes the form of a navigation filter only, with a state vetor ontaining the orretions to the navigation parameters. This way, the mapping and loalisation an e updated at low rates (1 to.5 Hz) and more omplete modelling of sensor errors is applied. While photogrammetry alone an solve SLAM, the presene of an IMU is signifiant for automated feature detetion. Moreover, the IMU is indispensale in ases where images annot e used for reasons of visiility and when ertain manoeuvres do not guarantee knowledge of the environment; e.g. when the root or vehile aptures images of an unknown environment. To use photogrammetry to solve SLAM, full automation is required, whih is still not fully ahieved. Many attempts are direted towards the automation of photogrammetry; this goal has een still falling short due to the need of high level of artifiial intelligene. In the next setion, the methodology to solve the SLAM prolem y photogrammetry and IMU is introdued. The third setion disusses riefly the photogrammetri mathematial model, the angular transformations and lever arm orretions that link the ameras outputs with those of the

3 IMU. A numerial investigation is desried and analysed in setion Four. Finally, onlusions are drawn in the last setion. 2. A Methodology for Comining Photogrammetry and IMU Outputs The methodology followed in this researh is different from the traditional SLAM solution pursued y the Rootis ommunity. In the following, there will e two separate filters; the first is the Least-squares Adjustment (LSA) filter that will do the mapping and loalisation y photogrammetry; the seond is the Kalman Filter that performs the loalisation y optimally integrating the loalisation provided y the LSA with the IMU outputs. The employed Kalman Filter (KF) is very similar to that used in the INS/GPS navigation. But instead of using the GPS positioning and veloity updates, the LSA outputs from photogrammetri resetion, the Exterior Orientation parameters (EOP), position and orientation, will e used as updates. In this way, the KF is a navigation-only filter that: i) operates at the frequeny of the update (e.g., 1 or.5 Hz), and ii) its state vetor size is kept relatively small (e.g., 15 states) with homogeneous states that guarantee rapid onvergene. This proedure requires ertain points to onsider: Reursive LSA: the LSA solution of the epoh k 1 is used as oservations for epoh k, Correlations etween measurements and unknowns are arried from one epoh to the other. Before starting, we need to initialise the system y defining its position and orientation with respet to a well-defined mathematial referene frame that has a physial meaning. This is important when the SLAM is solved on a gloal sale. This is alled Georeferening. The Georeferening an e done in two ways: 1. Initialisation with GPS/INS, whih demands open skies for the GPS signal, or 2. Initialisation with resetion, whih demands the existene of suffiient Ground Control Points (GCP) at the eginning of the survey. 3. Standing on a known point, and stale IMU alignment. In the first ase, open sky for the GPS is vital. The GPS/INS gives us the position and attitude of the IMU, whih after applying the lever arm and angles transformation yield the EOP of the two ameras. As for the seond ase, theoretially at least three GCPs are required for the determination of the position and attitude of the two ameras y resetion; however this is far from straightforward eause of the sensitivity of the IMU to misalignments. Experiene show that this is a very ritial aspet of SLAM and thus a suffiient numer of GCPs is required to e distriuted all of the images, otherwise weak initialisation fore the IMU to drift at the eginning of the mission. Having the initialisation properly done, the vehile moves and starts to map and loalise itself as portrayed in hart of Figure 1 that shows the algorithm onept. The data flow an e depited as follows: Initialisation: 1. Position and attitude of the two ameras onsidered as known 2. Intersetion is employed to map features

4 Known initial position Capture photos Initialisation Measure features photooordinates (x, y) and ompute their X, Y, Z y intersetion Move s seonds and apture photos Measure features photo-oordinates (x, y) of known X, Y, Z. Compute position and attitude of the two images y resetion 1 or.5 Hz 4 Hz Apply lever arm and oresight orretions IMU output Kalman Filter Output position and attitude Apply lever arm and oresight orretions Perform intersetion to map more features Yes More mapping? No STOP Figure 1: Flowhart of the Photogrammetri and IMU integration After mapping enough features: 1. Vehile moves. 2. Resetion omputes the ameras EOP y LSA using the features mapped from the previous ameras loation. (IMU predited EOP an e used for feature extration as well.) 3. Lever-arm and angles transformation (oresight) are applied to the EOPs to determine the IMU s position and attitude. 4. IMU outputs and IMU position and orientation derived from resetion are integrated in a KF to ompute filtered position and attitude of the urrent system loation. 5. Lever-arm and angles transformation (and oresight) are applied to the filtered position and attitude to determine the EOP of the ameras. 6. Intersetion is used to map more ojets y LSA from the urrent loation. 7. Vehile moves and algorithm repeats.

5 Stages 3 and 5 are of great importane in the proess of navigation and mapping, for the following fats (Figure 2): The ameras and the IMU are separate in spae The outputs of photogrammetry and IMU elong to different referene systems; photogrammetry funtions either in the amera spae or ojet spae and the IMU s outputs are in the ody frame. y y CCD x Z CCD x z X Y Figure 2: Mounting of the ameras and IMU 3. Photogrammetri Mathematial Model, Angle Transformation and Lever Arm Mathematial properties govern the relationship etween the image and the ojets. The perspetive entre, the ojet and its image are ollinear, yielding a funtional model alled the o-linearity equation: x = x y = y R R R R ( X X ) + R12 ( Y Y ) + R13 ( Z Z ) ( X X ) + R ( Y Y ) + R ( Z Z ) IMU ( X X ) + R 22 ( Y Y ) + R 23 ( ( X X ) + R ( Y Y ) + R ( Z Z Z Z where x, y are the photo-oordinates in the image frame, X, Y, Z are the 3-D Coordinates in the ojet frame, is the foal length of the amera, X, Y, Z are the 3-D Coordinates of the amera s perspetive entre in the ojet frame, x, y are the photo-oordinates of the projetion of the perspetive entre to the image plane, (theoretially, this projetion point has to oinide with the priniple point, whih is the entre of the image frame, ut in reality, it does not) and 's are the elements of the rotation matrix etween the image and ojet frames,. R ij Equations 1 desrie the fundamental mathematial model for photogrammetri mapping, where it reveals the relationship etween the image and the ojet oordinate systems. With this model, one an solve the asi prolems of photogrammetri mapping, namely: resetion and intersetion: Resetion: In resetion, the position and attitude (EOP) of an image are determined y having a set of at least three points with known oordinates in the ojet frame as well as in the image frame; these are the GCPs. Intersetion: In intersetion, two images, with known EOPs, are used to determine the oordinates in the ojet frame of features found on the two images simultaneously, employing the priniple of stereovision. ) ) z (1) R m

6 In priniple, photogrammetry alone an e used to solve SLAM y employing reursively resetion and intersetion. This was analysed y a previous puliation, Bayoud et al (24); however, there are important shortoming of suh approah as mentioned in the Introdution Angles transformation and lever arm The angle transformation applied in Stage 3 (going from resetion to KF), is used to transform the orientation output of the resetion from the mapping/amera frame to the earth/ody frame to e onsistent with the inertial output and KF states Where e e m ( R ) m T R = R T R (2) T is the rotation matrix etween IMU and amera frames and depends on the definition R of the axes (Figure 2), is the oresight that ontriutes for the mounting imperfetions, is the rotation matrix etween IMU ody frame and Earth-Centred-Earth-Fixed (ECEF) frame, and is the rotation matrix etween ECEF and mapping frames. e R m The angle transformation applied in Stage 5 (going from KF to intersetion) is used to transform the output of the inertial and KF to the amera s referene frame to perform the mapping. This is well doumented in the relevant literature eause it is the lassial way of image diret- Georeferening y GPS/INS (Skaloud and Shaer, 23). The transformation is: R m e T e ( R ) R m = T R (3) The lever arm is divided as well into two parts depending on whether the proess goes from resetion to KF or from KF to intersetion. In the first ase, resetion gives the oordinates of the ameras in the mapping frame. To these, the leverarm is added to otain the oordinates of the IMU in the mapping frame, and then to transformed to the ECEF frame. Having the oordinates of the IMU, omputed from resetion, in the ECEF frame, they update the KF to determine the filtered position of the IMU Leverarm and Boresight aliration R The onept of the oresight determination is well doumented in the relevant literature (Bäumker et al., 21; Skaloud and Shaer, 23). In this work, an indiret proedure was followed to determine the two oresight matries and two leverarm vetors of the left (L) and right (R) ameras. In the frame of the work arried out at the Geodeti Engineering Laoratory, a mapping system with a high-definition digital amera is well alirated with respet to the IMU with known oresight and leverarm; to this system, the two CCDs were added (Figure 3). The oresight and leverarm of the two CCDs were first alirated with respet to the high-definition digital amera y determining the EOP (y resetion from GCP) of the three ameras in three different loations. Then, one these were omputed, the link etween the two CCDs and the IMU were diretly made through the already known oresight and leverarm etween the digital amera and the IMU. e R

7 4. Numerial Appliation For testing the methodology and the set-up, two CCD ameras (progressive san SONY XC-55, 6448 square pixels of resolution 7.4 µm, and with a 6 mm lens) and an LN-2 IMU were used as instruments. Along, there were a synhronisation pulse, a Matrox Meteor-II/Multi- Channel frame graer and a sreen, IMU data aquisition ox developed at the EPFL-TOPO (Skaloud and Viret, 24), a laptop, and the power supply. The image graing was arried out at every seond and was properly synhronised with the IMU via a synhronisation pulse. After a ouple of minutes of inertial initialisation, the vehile moved and started taking images. See Figure 3. Figure 3: The system ontaining Sony CCD ameras, the LN-2 IMU, the High-resolution Hasselad amera used in aliration and a Laser sanner not used in this study 4.1. Controlled Environment Two tests were performed indoors in a ontrolled environment with the aim to evaluate the methodology and validate the software. In a ontrolled environment, auraies are very similar to the simulations. Two sets of images, eah ontaining 12 images in a 6 stereo-pairs with 3 to five seonds apart, were hosen. This means that the Kalman Filter update ours every three to five seonds. Figure 4 portrays sample images of the seond set. The initialisation was done using resetion, after whih, SLAM took over to map more features and loate the ameras. Aording to the photogrammetri theory and simulations, the depth (here a X & Y-omponents) is geometrially weak eause of the small stereo ase of 1 meter. Also, it is known that a large numer of GCPs is needed to determine an aurate EOP (8 to 1 GCP were used at eah epoh). If more GCPs were used, more aurate determination of the EOP would e guaranteed, and this in turn assures aurate intersetion. This was also onfirmed y these two tests. Four Chek Points (CHP) were used as ontrol points at the end of the survey. The positioning auray is indiretly ontrolled y the auray of the CHPs; for a diret ontrol on loalisation, an outdoors test with GPS is needed. Tales 1 and 2 show the differenes etween the CHP of the two tests, respetively, determined y a theodolite and their SLAM estimated positions. An interesting remark is the harmony in the signs of the differenes in the X and Y omponents; they are either positive or negative, while the Z omponent is more random (also the differenes in the Z- omponent an e onsidered as statistially insignifiant); this needs to e analysed in further tests to see if there is a trend or it is random ehaviour. CHP X Y Z CHP X Y Z

8 Tale 1: Validation of the first test, error on hek points after 2 seonds & 6 photos -m Tale 2: Validation of the seond test, error on hek points after 13 seonds & 6 photos - m 4.2. Outdoor test Figure 4: Image examples of the seond set The ameras used in the urrent set-up do not fit well the auraies needed; this is mainly due to their short foal length (6 mm) without enough pixels. In addition, the stereo-ase of 1 meter onstrains the intersetion geometry and limits the auray of the ojets at distane larger than 1 meters. This set-up is very sensitive to poor lightening and far features, whih need a lose to an ideal field. The initialisation was performed y determining the EOP y resetion. This proved to e a ritial phase due to the demand of high auray. Although the features were well distriuted and measured aurately y a totalstation, they were poorly seen on the images (Figure 5, first image); furthermore some of them were further than 15 meters from the ameras. This aused large misalignments of the IMU and in turn aused the predition to diverge as seen in Figure 7. However, due to the Kalman Filter updates, the INS solution is rapidly fixed in the third epoh and its preditions are onsistent with the resetion solution as an e seen in Figures 6 and 7. Figure 5: Outdoor images It is worthwhile to note the staility in the z-hannel as seen in Figure 7, where after the third epoh the innovation does not exeed few entimetres. This is due to the strong geometry of this hannel y photogrammetry as disussed aove. It is also laimed that the differenes in X & Y

9 (the innovation) are due to the poor positioning from the resetion; this analysis omes from the dedution that the Z-omponent of the IMU (its weak omponent) is lose to the Z-omponent of resetion (the strong photo omponent), so it is left that the differenes in Figure 6 are due to the inaurate resetion and not due to the IMU drift. Due to the short stereo-ase and poor feature reognition on the images the mapped features are determined with an auray of around 5 m; this in turn degrades the auray of the resetion that is dependent on the poor feature reognition as well. However, this means that the weighting in the Kalman Filter stohasti modelling needs more investigation. Figure 6: Differenes etween the predition and update in X & Y (innovation) Figure 7: Differenes etween the predition and update in Z (innovation)

10 5. Conlusions and Future Work Rootis SLAM an e solved y Geomatis modus operandi. An integration proedure etween an IMU and photogrammetry/ameras was developed and tested in this paper. The outputs of a pair of ameras are used first to loalise the vehile; this position is then used as an external measurements in a Kalman Filter whose predition are the filtered outputs of an IMU. The Kalman filtered position is used, then, with the outputs of the two ameras to perform the mapping in a Least-Squares adjustment filter. Coneptually, this integration is done in a loosely-oupled Kalman Filter. The algorithmi part of the filter, on the other hand, is far from simple and needs an understanding of system ontrol, data handling, and priority managing. This was done in the preliminary software that was written for the purpose of his work. The ontrolled numerial results showed that the methodology is promising and show the pratial feasiility of our proedure. However, the image omponent is ruial and its hoie has to e taken in are. Short foal length with low resolution highly limits the auray of the mapping that in turn degrades the quality of the vehile s loalisation. The short stereo-ase has as well a similar disadvantage. The oneptual advantages of using IMU/photo ompared to only photos SLAM are onsiderale: Image sequenes with limited or no overlaps do not halt the mapping IMU predited EOP allows automated feature extrations Inreased roustness and auray An important remark from this work is the need of synergy etween different speialisation for the advanement of speifi prolem. In this work two distint approahes were omined, from whih an apparent enefit for oth eome lear. Referenes: [1] Bäumker M., F. J. Heimes (21): New Caliration and Computing Method for Diret Georeferening of Image and Sanner Data Using the Position and Angular Data of an Hyrid Inertial Navigation System. OEEPE Workshop, Integrated Sensor Orientation, Sep , 21; Hanover. [2] Bayoud F. A., J. Skaloud, and B. Merminod (24): Photogrammetry-Derived Navigation Parameters for INS Kalman Filter Updates. XX th ISPRS Congress 24. Commission V, WG V/2, 24. [3] Csora M. (1997): Simultaneous Loalisation and Map Building. Ph.D. Thesis. Rootis Researh Group, Department of Engineering Siene, University of Oxford, [4] Newman P. M. (1999): On the Struture and Solution of the Simultaneous Loalisation and Map Building Prolem. Ph.D. Thesis, Australian Centre for Field Rootis, The University of Sydney, [5] Journal of Rootis Systems: Volume 21, Issues 1 and 2, January and Feruary 24, Wiley Periodials, pp 1-94.

11 [6] Jung I K (23): Simultaneous Loalisation and Mapping in 3D Environments with Stereovision. PhD Thesis. CNRS-LAAS Report N o 4132, Frane. [7] Skaloud J., P. Shaer (23): Towards a More Rigorous Boresight Caliration. ISPRS International Workshop on Theory, Tehnology and Realities of Inertial/GPS/Sensor Orientation, Commission 1, WG I/5, 23. [8] Skaloud J. and P. Viret (24): GPS/INS Integration: From Modern Methods of Data Aquisition to New Appliations, European Journal of Navigation, Nov. 24: 6-64.