Ground vehicle navigation in harsh urban conditions by ... - CiteSeerX

www.ietdl.org Published in IET Radar, Sonar and Navigation Received on 23rd March 2011 Revised on 10th July 2011 doi: 10.1049/iet-rsn.2011.0100

ISSN 1751-8784

Ground vehicle navigation in harsh urban conditions by integrating inertial navigation system, global positioning system, odometer and vision data S.-B. Kim1,5 J.-C. Bazin2 H.-K. Lee3 K.-H. Choi4 S.-Y. Park1 1

Department of Astronomy, Yonsei University, 262 Sungsanro, Seodaemungu, Seoul, South Korea Ikeuchi Lab, Institute of Industrial Science, The University of Tokyo, 4-6-1 Komaba, Meguro-ku, Tokyo 153-8505, Japan 3 Department of Electronic Engineering, Korea Aerospace University, 100 Hanggongdae-gil, Deokyang-gu, Koyang-city, South Korea 4 Department of Electronic Engineering, Mokpo National University, 61 Dorim-ri Chunggyem-yeon, Muan-gun, Cheonranamdo, South Korea 5 Division for Nuclear R&D Management, National Research Foundation, 180-1 Gajeongdong Yuseonggu, Daejeon, South Korea E-mail: [email protected] 2

Abstract: Combining GPS/INS/odometer data has been considered one of the most attractive methodologies for ground vehicle navigation. In the case of long GPS signal blockages inherent to complex urban environments, however, the accuracy of this approach is largely deteriorated. To overcome this limitation, this study proposes a novel ground vehicle navigation system that combines INS, odometer and omnidirectional vision sensor. Compared to traditional cameras, omnidirectional vision sensors can acquire much more information from the environment thanks to their wide field of view. The proposed system automatically extracts and tracks vanishing points in omnidirectional images to estimate the vehicle rotation. This scheme provides robust navigation information: specifically by combining the advantages of vision, odometer and INS, we estimate the attitude without error accumulation and at a fast running rate. The accurate rotational information is fed back into a Kalman filter to improve the quality of the INS bridging in harsh urban conditions. Extensive experiments have demonstrated that the proposed approach significantly reduces the accumulation of position, velocity and attitude errors during simulated GPS outages. Specifically, the position accuracy is improved by over 30% during simulated GPS outages.

1

Introduction

This section presents the background and previous research works, including an overview of multi-sensor ground vehicle navigation and an introduction to omnidirectional vision (OV). We also present the objectives and contributions of our approach. The section concludes with a preview of the proposed framework that will be detailed in the following sections. 1.1

Background and previous research works

Ground vehicle navigation aims to accurately estimate a vehicle’s position relative to its environment. Specifically, the integration of global positioning system (GPS) and inertial navigation system (INS) is considered an efficient sensor combination. The synergistic benefits might compensate for INS drift and bridge GPS positioning gaps caused by satellite signal interference [1 – 3]. However, the weakness of integrating GPS with INS is the heavy dependence on GPS signal availability. The accuracy of the system degrades sharply during GPS signal outage, which 814 & The Institution of Engineering and Technology 2011

is especially prevalent under heavy canopy or urban canyon environments. To overcome the INS accuracy degradation, several previous works have been proposed [4 – 12]. Among those methodologies, adding extra sensors is the most common approach. Some recent works also suggested a vision-based approach of the navigation problem by embedding vision sensors [13–18]. Cameras can perform various missions such as recognition and target tracking. As widely known [13–15], vision sensors can provide periodic updates to the inertial sensors and compensate for drifts because of the bias errors inherent in INS system for position, velocity and attitude (PVA) estimates. Since a vision sensor usually has update rates and error properties different from the other sensors, an extended Kalman filter is generally used to optimally integrate the sensor information into a navigational solution [19, 20]. A Kalman filter is a powerful method to improve PVA estimation and reduce the effect of sensor drift. To combine outputs from multiple sensors efficiently, an appropriate Kalman filter needs to be designed. The design of a multisensor system requires both a system model and a measurement model. The system model describes how the IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

www.ietdl.org state variables behave without external measurements. The measurement model describes how the measurements are related to the state variables. Depending on whether the unmodified variables or the error variables are utilised as the states, a Kalman filter is categorised as a direct or indirect type [20]. Indirect Kalman filter is generally preferred since the dynamics of error variables are much slower than those of direct variables avoiding complex and frequent non-linear time propagations. We design an indirect Kalman filter to perform the multi-sensor integration and filter the measurement noise from vision and inertial sensors. However, most existing vision-based approaches suffer from some limitations, such as rotation/translation ambiguity and a small field of view. Therefore much work has been done to mitigate these limitations [15, 21– 24]. In spite of these efforts, most current techniques are based on complex mathematical models or assume a low dynamic environment. Accurate and reliable land vehicle navigation in harsh urban conditions is still considered a challenging problem and therefore significant improvements need to be achieved for practical applications. 1.2

Objectives and contributions

This paper attempts to improve ground vehicle navigation accuracy in harsh urban conditions where GPS signals are often blocked and deflected. The attitude of a vehicle refers to its orientation with respect to a defined reference coordinate system. For simplicity, the terms ‘attitude’, ‘orientation’ and ‘rotation’ are used equivalently in the remaining of this paper. In a kinematic inertial model, accurate attitude is fundamental to determine the position of a vehicle in dynamic motion. However, the bias error and random walk noise from attitude sensors will accumulate and cause non-linear errors leading to a degradation of the position accuracy. Hence we propose a novel and reliable ground vehicle navigation system in which stable and robust integration mechanism provides seamless data rates and no accumulation of the attitude error by the combination of a GPS receiver, an INS, an odometer and an OV sensor. A vision sensor can provide long-term accurate attitude information, as will be shown in Section 4, and has a relatively low frame rate, typically around 20– 30 frames

per second. In contrast, the INS deteriorates the navigation accuracy very rapidly with time growth and runs at very high rate, around several hundreds of Hertz. By combining the complementary advantages of INS and vision systems, a stable and robust integration mechanism can be implemented featuring high data rates, like INS, and without attitude error accumulation from vision sensor [15, 16]. Omnidirectional cameras, by definition, are cameras that provide a 3608 field of view of the scene. Among the different kinds of OV sensors, the polydioptric camera bears the most attractive characteristics since it can provide a wide field of view, with the highest resolution and best focused/sharp images. The polydioptric camera refers to a cluster of synchronised cameras whose pictures are stitched together to build a panoramic image [25]. Polydioptric cameras are gaining popularity for diverse robotic applications and our study concentrates on this polydioptric case. Our approach stems from the idea that attitude information can be determined by vanishing points (VPs) in visual data. VPs refer to the intersection of the projection of world parallel lines in the image. By following this idea, VPs are extracted, stored and tracked during a long omnidirectional image sequence. Then, the absolute rotation between any two frames can be computed without error accumulation. Finally, the accurate attitude information from the OV sensor is augmented in the GPS/INS/odometer integration system. The two types of measurements, namely from the odometer and the OV sensor, are fed back into the Kalman filter to mitigate the accuracy degradation induced by GPS signal blockages. The block diagram in Fig. 1 illustrates our proposed concept for an OV-aided GPS/INS/odometer system. The vehicle travels equipped with GPS receivers, inertial measurement unit (IMU), an odometer and an OV sensor. As long as GPS measurements are available, the performance and the accuracy of the GPS/INS/odometer are usually satisfactory [8 – 10]. However, when GPS measurements are unavailable, the traditional INS/odometer navigation solution drifts with time because of its inherent bias. Therefore the vision-based measurement update is accomplished by attitude estimation from the VPs in the omnidirectional images.

Fig. 1 Proposed sensor fusion processing flowchart by combining GPS/INS/odometer and OV data for ground vehicle trajectory estimation When GPS outage occurs, the measurements from OV sensor as well as odometer are combined with INS data in the Kalman filter to provide navigation solution IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

815

& The Institution of Engineering and Technology 2011

www.ietdl.org The input measurements, that is attitude from OV sensor as well as odometer data, are combined with INS data in the Kalman filter to determine error states. These error states are then used to correct and calibrate the navigation solution from the INS output. Our OV-aided scheme provides several additional advantages and contributions. First, our approach requires no a priori knowledge about the environment. Second, our approach provides stable position and attitude information with high data rates and significant no error accumulation. Third, it overcomes the limitations inherent to the small field of view of traditional cameras by utilising an OV system. As will be shown in the remaining sections of this paper, these advantages improve the quality of INS drifts during GPS signal blockages. The organisation of the remainder of this paper is as follows. In Section 2, we explain how to extract VPs in omnidirectional images and compute the orientation of the vehicle. In Section 3, the proposed integration scheme combining GPS, INS, odometer and omnidirectional camera is explained in details. In Section 4, experimental results are provided to demonstrate the validity and the robustness of the proposed approach. Finally, concluding remarks are given in Section 5.

2 Attitude estimation from omnidirectional images Compared to conventional cameras, OV sensors have a much wider field of view. The great impact for our application is that numerous parallel lines can be observed simultaneously, which allows to detect VPs and, in turn, estimate the vehicle’s attitude. This section explains how to extract VPs in omnidirectional images and how the attitude angles can be estimated from these VPs. The first subsection is dedicated to the line detection algorithm. VP extraction and attitude estimation are presented in the second subsection. 2.1 Line detection algorithm in omnidirectional images In this paper, we extract VPs in omnidirectional images and compute the attitude angles from these extracted VPs. We follow the approach of Bazin et al. [26, 27]. Many parallel lines exist in a man-made environment. VP refers to the point at which world parallel lines seem to converge in an image because of perspective projection. Each set of parallel lines is associated to a VP. The VPs provide key information for attitude estimation and thus can play a crucial role for estimating a vehicle’s orientation and motion. A preliminary step to estimate attitude information from omnidirectional images is the detection of lines. The line detection method is based on the great circle constraint: a great circle is defined as the intersection between a sphere and a plane passing through the sphere centre, as depicted in Fig. 2a. This constraint implies that a world line is projected onto the equivalent unit sphere as a great circle. The procedure for line detection is summarised as follows. First, it begins with detecting edges in omnidirectional images and building chains of connected edge pixels. Then these chains are projected onto the equivalent sphere. If a chain verifies the great circle constraint, it is considered a line. Otherwise, it is cut into two sub-chains and the procedure iterates until a sub-chain is considered a line or 816

& The Institution of Engineering and Technology 2011

Fig. 2 Projection of a 3D world line model and the equivalent sphere as a great circle a Illustration of a great circle where Oc is the sphere centre in the projection model, and (nx , ny , nz) is the normal vector b Cut of a spherical chain (shown in green) during the splitting step of the line detection algorithm

its length is too small. This process is called a `‘splitting step’ and is illustrated in Fig. 2b. This step may perform a multi-detection of one single line because of occlusion or edge detection. Therefore a procedure called `‘merging step’ is applied to gather the sub-chains corresponding to a same world line. The output consists of a list of great circle normals in the sphere space, where each normal corresponds to a detected line. Additional technical details can be found in [27]. Typical results for omnidirectional images acquired by Point Grey’s ladybug camera are illustrated in Fig. 3. Especially, an example of the line splitting and merging steps is shown in Figs. 3c and d.

2.2

VP extraction and attitude estimation

Once the lines have been detected (cf. Section 2.1), the next procedure is to extract the associated VPs and compute the attitude angles. As reviewed in [28], most of the existing works conduct the following procedure: first, cluster the detected lines in sets of parallel lines; second, compute the VPs; and third, estimate the attitude angles. This traditional approach suffers from some limitations such as computationally expensive, non-orthogonality of the VPs, no use of some a priori attitude values [28 – 30]. Especially, they cannot guarantee the VP orthogonality because they extract the VPs independently and subsequently compute the attitude (‘bottom-up’ approach). Since our application takes place in urban environment, the dominant vanishing directions are usually orthogonal. Thus the orthogonality of VPs must be imposed along the whole image sequence. To overcome the limitations associated with the bottom-up approach, we follow the top-down approach of [26]. The term ‘top-down’ refers to the fact that it calculates the attitude angles in a reversed way, compared to traditional methods: first, some attitude hypothesis are built (obtained by motion model or heuristic), and subsequently, the consistency of the VP extraction is evaluated. The (hypothesised) attitude angles maximising the VP consistency correspond to the estimated attitude of the vehicle. This top-down scheme can be formulated as

arg max f,u,c

3  N 

d(Rf,u,c ei , nj )

(1)

i=1 j=1

IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

www.ietdl.org

Fig. 3 Examples of line extraction a b c d

Edge detection using Canny detector Chaining result Line detection result after splitting step Line detection result after merging step

with 

d(u, v) =

1 0

 p   if arccos(u · v) −  ≤ t 2 otherwise

where f, u, c correspond to the attitude angles (roll, pitch and yaw), Rf,u,c is the associated rotation matrix, hj represents the normal associated to the jth detected line in the sphere space (cf. Section 2.1), N is the number of detected lines and ei represents the ith axis of the Cartesian coordinate system [i.e. ei ¼ (1, 0, 0)]. Equation (1) aims to find the attitude angles maximising the VP consistency which is defined as the number of lines passing through the current VPs. For that, the hypothesised attitude Rf,u,c transforms an original orthonormal coordinate system (e1 , e2 , e3) into a new orthonormal basis (v1 , v2 , v3) where vi ¼ Rf,u,c , ei . If the rotation Rf,u,c coincides with the camera orientation, this new basis will be aligned with the dominant directions of the image. To count the number of lines passing through the VPs (v1 , v2 , v3), we use a Dirac function that computes the angle between each line, represented by its normal n, and each VP vi ¼ Rf,u,c , ei for i ¼ 1, 2, 3. When the maximisation of (1) is completed, we obtain † the attitude angles ( f, u, c) and the associated rotation matrix (Rf,u,c); † the cluster of parallel lines passing through the detected VPs; † the three VPs vi ¼ Rf,u,c , ei representing the dominant directions of the image. It should be noted that they are mutually orthogonal by construction, since the basis vectors (e1 , e2 and e3) are orthogonal to each other. Absolute attitude is usually computed by composing/ integrating all the rotations from the first to the current image. Each image might involve some inaccuracies, which, in turn, can lead to the accumulation of attitude error over time. On the contrary, it is interesting to note that the rotation Rf,u,c at function (1) is computed with respect to a fixed and global coordinate system (e1 , e2 and e3). IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

Therefore our proposed approach permits to estimate the attitude of the current frame directly, without any composition/integration, which prevents from error accumulation. To solve function (1), we apply a multi-scale sampling approach [26]: the discretisation of the rotation angles is performed until a desired precision is reached, given an initial solution. Instead of using the results of the previous frame as an initial solution, one might prefer to estimate the rotation at the current frame using a dynamic model. This approach reduces the complexity of the calculation and is more robust to sudden rotation. This dynamic model can be mathematically written as qt = qt−1 ⊗ q((V + vV )Dt )

(2)

where q is the quaternion representation of rotation R, ⊗ represents the quaternion multiplication, V is the angular velocity, Dt is the time interval between t 2 1 and t and vV represents some velocity noise. Fig. 4 depicts typical results of VPs extraction for omnidirectional images. They represent the three dominant VPs and their associated parallel lines. Each conic represents a detected line and is clustered to the set of parallel lines it belongs to. It shows that the VPs, represented by small dots in Fig. 4, can be robustly extracted and continuously tracked along the whole sequence despite large camera motion.

3 Integration algorithm using OV-aided INS, odometer and GPS Integrating GPS, INS, odometer and OV provides a suitable alternative for GPS-denied environments. This integration scheme has to cope with a major limitation related with the frequent GPS outages. In such an environment, the attitude error is considered the largest INS error component [31, 32] since it has a huge impact on the INS navigation performance. This section explains how OV is utilised to enhance the accuracy of INS navigation. Our scheme for 817

& The Institution of Engineering and Technology 2011

www.ietdl.org

Fig. 4 VP extraction in omnidirectional image sequence Each conic represents a detected line and is also clustered to the set of parallel lines it belongs to. The VP is displayed by a small dot

the OV-aided GPS/INS/odometer data fusion is presented, with also the details of a system model and an observation model using the Kalman filter. 3.1 Design of the system matrix for OV-aided GPS/ INS/odometer integration The most important components in an IMU are accelerometers and gyroscopes. Accelerometers and gyroscopes are sensors that provide specific force and angular velocity measurements, respectively. These measurements are used to calculate the vehicle’s PVA mechanisation by the following differential equation ⎛

⎞ ⎛ ⎞ r˙ v l b l l l ⎝ v˙ ⎠ = ⎝ Rb f − (2vie + ven ) × v + g ⎠ l R˙ b Rlb vb

(3)

where r is the position vector in local-level frame, v is the velocity vector in local-level frame, f b is an acceleration vector acquired from IMU, vb is a gyro measurement vector acquired from IMU, g is the gravitation vector, vlie represents the Earth rotation rate expressed in local-level frame, vlen is the transport rate, Rlb is a transformation matrix from the body frame to the local-level frame (please refer to the Appendix for coordinate frame definition) and the superscript represents the derivative with respect to time. This equation provides the necessary navigation solutions for rigid body motion in a three-dimensional local-level frame. However, this solution is vulnerable to the error factors that cause severe trajectory deviation and sensor drift. The errors include drifts, scale factors and nonorthogonality of the sensors. In order to combine INS with other extra sensor measurements like GPS and odometers, some of the error factors should be considered. The INS error models are obtained by linearising the INS mechanisation equations, and they include position, velocity and attitude errors. The detailed basic linearised INS error models are derived in [32, 33]. Based on the INS error models, attitude error and accelerometer bias are the main contributors to induce velocity and position errors. In addition, it should be noted that the growth of velocity errors does not quickly translate into growth of attitude error. However, the growth of attitude error can 818 & The Institution of Engineering and Technology 2011

significantly affect and degrade the velocity and position. For this reason, the gyro drift rate is an indicator of quality of INS performance. Thus, it would be very beneficial to provide the attitude and velocity measurements from extra sensors, such as OV sensors, during GPS outages. By providing attitude measurements, in general, the roll, pitch and yaw angles become directly observable. This, in turn, makes it possible for a Kalman filter to discriminate the vertical gyro drifts, which is one of the most difficult states to estimate. In our case the velocity measurement, combined with the fully observable attitude errors and the gyro drifts, helps to discriminate the velocity error and the accelerometer biases by using the Kalman filter. In this way, the velocity, attitude and inertial sensor errors can be estimated by indirect compensation. Since the velocity error can be bounded, the position error growth can also be bounded. For our scheme, the basic INS error model augmenting OV sensor error states can be expressed to construct the Kalman filter as follows

dx(k + 1) = F(k)dx(k) + G(k)v(k)

(4)

where ⎛

Fins F = ⎝ O3×15

⎞ O15×3 I3×3 ⎠

O15×3 is a 15 × 3 zero matrix, O3×3 is a 3 × 3 zero matrix and I3×3 is a 3 × 3 identity matrix. The sub-matrix Fins is summarised in [33 – 35]. The estimated state errors dx, which can be obtained from the Kalman filter, are defined as

dx = [dxnav , dxsensor , dximg ]T dxnav = [dL, dl, dh, dvn , dve , dvd , fn , fe , fd ]T dxsensor = [dfxb , dfyb , dfzb , dvx , dvy , dvz ]T dximg = [dxdrift , dydrift , dzdrift ]T They are fed back to the inertial navigation solution in order to correct the contaminated navigation estimates as indirect IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

www.ietdl.org feedback implementation. In our approach, the Kalman filter state variables are the position errors (dL, dl, dh), the velocity errors (dvn , dve , dvd), the attitude errors (dfn , dfe , dfd), the gyro drifts (dvx , dvy , dvz), the accelerometer biases (dfxb , dfyb , dfzb ) and the OV sensor errors (dxdrift , dydrift , dzdrift).

4

3.2 Design of the observation matrix considering misalignment

4.1 Experiment set-up and trajectory data acquisition

The Kalman filter compares the INS output with data acquired from OV and odometer. Therefore two types of measurements can be utilised for the measurement update of the Kalman filter during GPS outages: the attitude derived from the OV sensor and the speed derived from the odometer. Hence, the Kalman filter measurement equation utilised during GPS blockages can be modelled as follows

The field trial was conducted in Daejeon, South Korea, and the hardware architecture is illustrated in Fig. 5. The OV camera used in our experiments is the Point Grey’s Ladybug. The Ladybug camera is a typical example of polydioptric device that is composed of six colour CCD sensors, each collecting 1024 × 768 images at a maximum frame rate of 30 fps [25]. The Ladybug camera and a medium-grade LN-200 IMU are installed on the roof of the van and an odometer is embedded at the rear wheel of the van to record the distance travelled. The positioning and orientation system consists of a pair of Trimble GPS receivers, mounted on the front and back of the roof. The camera is triggered by the control software based on the distance travelled by the vehicle (e.g. every 1 m). They are synchronised to the GPS time through the exposure signal sent to the input event port of the receiver. The odometer provides the forward direction (X-direction) velocity in the vehicle frame, whereas two non-holonomic constraints are applied to the Y and Z directions of the vehicle body frame. The non-holonomic constraints imply that the controllable degrees of freedom are less than the total degrees of freedom. When it comes to ground vehicle navigation, the vehicle motion in the plane perpendicular to the forward direction is assumed to be zero, although vehicle bumping and sliding might violate this assumption. Most of the previous researches related to the integration of odometer sensor information with GPS/INS applied two non-holonomic constraints [9, 10, 39]. We followed the same approach to obtain an auxiliary velocity update. The measurement data from a dual-frequency GPS receiver, LN200 IMU and an odometer were post-processed to estimate navigation states. The reference trajectory shown in Fig. 6 was generated by the commercial software POSPac from the Applanix Corporation. POSPac computes the optimally accurate backward smoothed navigation solution. The reference estimation accuracy from software indicates that the solution is around subcentimetre accuracy under a good GPS constellation [37]. Throughout the experiments, more than four satellites were visible, except during several GPS signal outages caused by bridges or trees.

Z=

Vins Ains



−

Vodo Avis



= Hvo

xins ximg

+ vvo

(5)

where Z denotes the velocity and attitude difference. Vins is the velocity vector with respect to the local-level (l ) frame provided by the strapdown INS (SDINS), Vodo represents the velocity vector with respect to the l-frame provided by the odometer, Aini is the three-dimensional vector consisting of the roll, pitch and yaw angles provided by the SDINS, and Avis denotes the three-dimensional vector consisting of the roll, pitch and yaw angles provided by the OV sensor. The observation matrix Hvo is defined as Hvo =

O3×3 O3×3

I3×3 O3×3

Rlb Vblb rb Rcalib

O3×3 O3×3

−Rlb rb × O3×3 O3×3 O3×3



where the subscript vo stands for ‘vision and odometer’, rb is the offset vector from the INS centre to the odometer centre with respect to b-frame, Rlb represents the rotation matrix from the b-frame to the l-frame, Rcalib is the small misalignment between gyro sensor and OV sensor and Vbib is the skew-symmetric rotation matrix that can be constructed by the outputs of the strapdown gyroscopes. A small misalignment between the gyro sensor and the OV sensor are inevitable [35, 36]. For the utilisation of the OV sensor, we should consider this misalignment denoted Rcalib . The estimation of Rcalib is performed in such a way to minimise the following formula [26]

Rcalib = arg min R

= arg min q

N 

||vlj − R(q)vcj ||2

N 

(qvlj qT )vcj

(6)

j=1

where vlj is the jth gravity direction measured by gyroscope and expressed with respect to the local-level frame, vcj is the corresponding vertical direction measured by the OV sensor and expressed with respect to the camera sensor frame (c), R(q) denotes the rotation matrix based on the quaternion q from the c-frame to the l-frame, and N represents the number of observed vertical vectors. IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

In this section, field test results are presented to verify the feasibility of the proposed OV-aided sensor integration approach. The experimental setup and detailed results during simulated GPS outages are presented.

4.2

j=1

Experimental results

Performance analysis during GPS outages

To evaluate the performance of the proposed system during GPS outages, intentional GPS outages were injected for 100 s into the measurements collected around the area of the oval-shaped route in Fig. 6b. This area was deliberately selected to induce a large error growth rate. During GPS outages, the error accumulation rate is, in general, heavily determined by the Kalman filter which is calibrated before the GPS signals are blocked. In case of favourable GPS constellation, centimetre-level accuracies can be achieved by using carrier phase measurements whereby the integer ambiguities are resolved correctly. 819

& The Institution of Engineering and Technology 2011

www.ietdl.org

Fig. 5 System architecture and hardware description a Van acquisition platform equipped with an OV sensor (Ladybug camera), GPS, INS and odometer b Control system used to synchronise and store the acquired data

Fig. 6 Vehicle trajectory for the experiment a Reference trajectory obtained by POSPac and displayed in Google Earth b Its detailed 2D trajectory (latitude and longitude) along with DGPS points

Fig. 7 compares the reference attitude obtained by POSPac and the attitude estimated from visual data. We obtain a mean error of 0.198 for roll angle, 0.228 for pitch angle and 0.938 for yaw angle, which demonstrates the robustness and accuracy of our system. As explained in Section 3.2, the camera-INS calibration method is based on the observations of the vertical direction or gravity with respect to the navigation frame and the vision sensor frame, respectively. Fig. 7a (against Fig. 7b) clearly shows the effects of the calibration method. The Kalman filter performance was evaluated using covariance analysis since the covariance matrix indicates the theoretical filtering performance. Fig. 8 shows the rootmean-square values of the covariance of the attitude error states derived from the diagonal elements of the estimated state covariance matrix. This figure shows that the estimation precision is improved by inclusion of the visionbased attitude information. It also illustrates the important 820 & The Institution of Engineering and Technology 2011

fact that the improvement is especially significant when the vehicle undergoes GPS outages. Without the OV sensor aiding as in the conventional approach, roll and pitch errors quickly converge, whereas heading or yaw errors converge very slowly as shown in Fig. 8a. Through the observability enhancement when the vehicle changes its direction, the heading error can be reduced. By the aid of the OV sensor, as shown in Fig. 8b, the heading error also converges very quickly and its growth is limited even during GPS outages. Of primary importance, heading accuracy is predicted to have a standard deviation as small as 0.188. For sensor bias estimation performance in Kalman filter, Fig. 9 compares the standard deviation values of the conventional and proposed approaches. From Fig. 9a, vertical gyro drift has poor estimation performance by the conventional GPS/INS/odometer integration (e.g. poor observability). In contrast, Fig. 9b shows that the proposed scheme improves the estimation performance of vertical IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

www.ietdl.org

Fig. 9 Covariance results for the gyroscope error state: standard deviation of gyro error Fig. 7 Comparison of the attitude angles obtained from POSPac (reference data) and from omnidirectional images

a By the conventional GPS, INS and odometer b By the proposed OV-aided GPS, INS and odometer

a Before camera-INS calibration b After camera-INS calibration

Fig. 8 Covariance analysis comparison for the attitude error state: standard deviation

Fig. 10 Positioning error during the experiments (including nonGPS simulation section)

a By GPS, INS and odometer b By the proposed OV-aided GPS, INS and odometer

a For general GPS, INS and odometer b By the proposed OV-aided GPS, INS and odometer

IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

821

& The Institution of Engineering and Technology 2011

www.ietdl.org Table 1

Performance statistics during GPS outage INS/odometer

north, m east, m down, m 2D, m

Mean

Deviation

Max

Mean

Deviation

Max

0.20 1.62 0.02 1.63

0.13 0.98 0.019 0.99

0.73 3.11 0.08 3.19

0.42 0.99 0.03 1.13

0.28 0.63 0.017 0.67

0.94 2.07 0.07 2.23

gyro drift. Thus it indicates that strongly observed states (e.g. the position) converge quickly, whereas weakly observed states (e.g. sensor biases) require more time to converge. If all the states have not converged, the estimation would have led to poor results. Fig. 10 depicts the East, North and height position errors by the OV-aided INS/odometer. During the initial stage of the dataset where GPS visibility is good, it is shown that the conventional and proposed approaches provide sub-metre level accuracy. During GPS outages, the proposed approach has the capability of maintaining a 1 – 2 m accuracy. Such accuracy might be considered very large; however, it can be seen that the position error is smaller than that of the conventional system because of the aid of the OV sensor. To reach sub-metre accuracy, more considerations could be given to the misalignment calibration between the OV device and the inertial sensor, which is left for future research. Table 1 compares the positioning error of the conventional approach and the proposed approach during the GPS outage, which corresponds to the ellipse area in Fig. 6b. These very large errors occur during an extended period of limited GPS measurements and also because the filter has not had enough accurate vision measurements to estimate the inertial sensor errors sufficiently compared to GPS measurements. As shown in Table 1, the standard deviation values of the north and east errors during GPS outage are approximately 0.28 and 0.63 m, respectively. In fact, the maximum east positioning error reached 3.11 m for conventional INS/ odometer, whereas the maximum error for our vision-aided approach was only 2.07 m, which occurred at the end of the GPS outages during the experiment. Compared to the conventional approach, our proposed scheme provides about 30.7% (1.63 to 1.13), 31.6% (0.99 to 0.67) and 30.2% (3.19 to 2.23) improvement in the average, deviation and maximum of the horizontal position error, respectively. These results demonstrate that the proposed OV-aided approach is suitable for land navigation in dense urban conditions where frequent GPS signal outages are expected. In fact, the accuracy is high enough so that the error in the reference solution from the POSPac could affect the error statistics. Overall, it can be seen that the proposed OVaided approach maintains the position accuracy within a few metres even after a 100 s gap when the vehicle is travelling under GPS outage environments.

5

Proposed OV aided

Conclusions

The goal of this research has been to improve the accuracy of ground vehicle navigation in GPS denied environments. Our approach has integrated an OV system into the conventional ground vehicle navigation system (GPS, INS and odometer). By incorporating OV data, a ground vehicle navigation mechanism can be implemented even during GPS outages. 822

& The Institution of Engineering and Technology 2011

The contributions of this study can be summarised as follows. First, our approach requires no a priori knowledge about the environment, which can avoid expensive costs and complicated inverse trajectory projection problems. Second, by combining the complementary advantages of INS and vision devices, our approach provides stable and robust position and attitude information featuring high data rates (from INS) and no attitude error accumulation (from vision). Third, our study uses OV sensors, which are appropriate to detect and track features. Finally, our study uses OV sensors that can overcome the shortcomings of the limited field of view of traditional cameras. Extensive experiments have demonstrate that the proposed OV-aided approach, first, provides position errors bounded for long GPS outages, and second, outperforms the conventional approach during that time. Therefore the OVbased attitude determination is a valuable external aid for ground navigation systems. Combined with the odometerbased speed information, the proposed scheme is suitable for navigating in urban areas where the signal from the satellites is blocked or interfered with buildings or trees. This results in a robust positioning solution provider that can reliably operate in challenging urban conditions by combining distinct sensors and technologies. In future work, we plan to study the use of low-cost and off-the-shelf sensors in order to decrease the equipment cost and easily install it on commercial cars.

6

Acknowledgments

This work was partly supported by Priority Research Centers Program (2010-0028295) funded by the Ministry of Education, Science and Technology. This work was also supported by National Strategic R&D Program for Industrial Technology, South Korea. Dr. Young-Eun Joo, Mr. Jun-Seok Choi and Mr. Hong-Bum Cho with ENG company are specially acknowledged for providing the test dataset.

7

References

1 Farrell, J., Barth, M.: ‘Global positioning system and inertial navigation’ (McGraw-Hill, 1999) 2 Titterton, D.H., Weston, J.L.: ‘Strapdown inertial navigation technology’ (Peter Peregrinus Ltd., 1997, 2nd edn.) 3 Islam, A., Iqbal, U., Langlois, J.M.P., Noureldin, A.: ‘Implementation methodology of embedded land vehicle positioning using an integrated GPS and multi sensor system’, Integr. Comput.-Aided Eng., 2010, 17, (1), pp. 69–83 4 Bayoud, F.A.: ‘Vision-aided inertial navigation using a geomatics approach’. Proc. ION GNSS-2005, Long Beach, USA, September 2005, pp. 2485–2493 5 Dissanayake, G., Sukkarieh, S., Nebot, E.: ‘The aiding of a low-cost strapdown inertial measurement unit using vehicle model constraints for land vehicle applications’, IEEE Trans. Robot. Autom., 2001, 17, (5), pp. 731– 747 6 Hasan, A.M., Samsudin, K., Ramli, A.R., Azmir, R.S.: ‘Comparative study on wavelet filter and thresholding selection for GPS/INS data fusion’, Int. J. Wavelets Multiresolution Inf. Process., 2010, 8, (3), pp. 457–473 IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

www.ietdl.org 7 El-Sheimy, N., Nassar, S., Shin, E.-H., Niu, X.: ‘Analysis of various kalman filter algorithms using different inertial navigation systems integrated with the global positioning system’, Can. Aeronaut. Space J., 2006, 52, (2), pp. 59– 67 8 Limsoonthrakul, S., Dailey, M.N., Parnichkun, M.: ‘Intelligent vehicle localization using GPS, compass, and machine vision’. IEEE RSJ, Missouri, USA, October 2009, pp. 3981–3986 9 Pingyuan, C., Tianlai, X.: ‘Data fusion algorithm for INS/GPS/odometer integrated navigation system’. IEEE ICIEA, Harbin, China, May 2007, pp. 1893– 1897 10 Rezaei, S., Sengupta, R.: ‘Kalman filter-based integration of DGPS and vehicle sensors for localization’, IEEE Trans. Control Syst. Technol., 2007, 15, (6), pp. 1080– 1088 11 Bayoud, F.A., Skaloud, J.M., Merminod, B.: ‘Photogrammetry derived navigation parameters for INS Kalman filter updates’. Proc. XXth ISPRS Congress, 2004 12 Noureldin, A., El-Shafie, A., El-Sheimy, N.: ‘Adaptive neuro-fuzzy module for inertial navigation system/global positioning system integration utilising position and velocity updates with real-time crossvalidation’, IET Radar Sonar Navig., 2007, 1, (5), pp. 388–396 13 Madison, R., Andrews, G., Debitetto, P., Rasmussen, S., Bottkol, M.: ‘Vision-aided navigation for small UAVs in GPS-challenged environments’. AIAA InfoTech at Aerospace Conf., 2007, pp. 2733–2745 14 Kim, J.H., Ridley, M., Nettleton, E., Sukkarieh, S.: ‘Real-time experiment of feature tracking/mapping using a low-cost vision and GPS/INS system on an UAV platform’, J. Glob. Posit. Syst., 2004, 3, (1), pp. 167–172 15 Goldshtein, M., Oshmant, Y., Efrati, T.: ‘Seeker gyro calibration via model-based fusion of visual and inertial data’. ISIF, Quebec, Canada, July 2007 16 Olson, C.F., Matthies, L.H., Schoppers, M., Maimone, M.W.: ‘Rover navigation using stereo ego-motion’, Robot. Auton. Syst., 2003, 43, (4), pp. 215– 229 17 Veth, M.M., Raquet, J.: ‘Fusion of low-cost imaging and inertial sensors for navigation’. ION GNSS, 2006, vol. 2, pp. 1093– 1103 18 Saeedi, S., Samadzadegan, F., El-Sheimy, N.: ‘Vision-aided inertial navigation for pose estimation of aerial vehicles’. ION GNSS, Savannah, GA, September 2009, pp. 453– 459 19 Maybeck, P.S.: ‘Stochastic models, estimation and control’ (Academic Press, 1979) 20 Brown, R.G., Hwang, P.Y.C.: ‘Introduction to random signals and applied Kalman filtering’ (John Wiley Sons Inc., 1997) 21 Ma, Y., Soatto, S., Koseck, J., Sastry, S.S.: ‘An invitation to 3-D vision’ (Springer, 2004) 22 Lopez, M.R., Sergiyenko, O., Tyrsa, V.: ‘Machine vision: approaches and limitations’, in Zhihui, X. (Ed.): ‘Computer vision’ (In Tech, 2008, 1st edition), pp. 395– 428 23 Scaramuzza, D.: ‘Omnidirectional vision: from calibration to robot motion estimation’. PhD thesis, University of Perugia, 2008 24 Banno, A., Ikeuchi, K.: ‘Omnidirectional texturing based on robust 3D registration through Euclidean reconstruction from two spherical images’, Comput. Vis. Image Underst., 2010, 114, (4), pp. 491–499 25 Pointgrey Research: ‘Overview of the Ladybug image stitching process’. Technical Application Note, 2008 26 Bazin, J.C., Kweon, I.S., Demonceaux, C., Vasseur, P.: ‘A robust top down approach for rotation estimation and vanishing points extraction by catadioptric vision in urban environment’. IROS, Nice, France, September 2008, pp. 346–353

IET Radar Sonar Navig., 2011, Vol. 5, Iss. 8, pp. 814 –823 doi: 10.1049/iet-rsn.2011.0100

27 Bazin, J.C., Demonceaux, C., Vasseur, P., Kweon, I.S.: ‘Motion estimation by decoupling rotation and translation in catadioptric vision’, Comput. Vis. Image Underst., 2010, 114, (2), pp. 254–273 28 Matthew, E.A., Seth, J.T.: ‘Automatic recovery of relative camera rotations for urban scenes’. Proc. IEEE CVPR, Hilton Head, USA, June 2000, pp. 2282–2289 29 Magee, M.J., Aggarwal, J.K.: ‘Determining vanishing points from perspective images’, Comput. Vis. Graph. Image Process., 1984, 26, (2), pp. 256– 267 30 Quan, L., Mohr, R.: ‘Determining perspective structures using hierarchical hough transform’, Pattern Recognit. Lett., 1989, 9, (4), pp. 279–286 31 Luiz, G.B.M., Jorge, D.: ‘Exploiting attitude sensing in vision-based navigation for an airship’, J. Robot., 2009, 2009, pp. 1– 16 32 Kim, S.B., Lee, S.Y., Kim, M.S., Lee, J.H.: ‘The design of DGPS/INS integration for implementation of 4S-Van’, J. Astron. Space Sci., 2002, 19, (4), pp. 351–366 33 El-Sheimy, N.: ‘Inertial techniques and INS/DGPS integration’. Course Note from University of Calgary, 2007 34 Shin, E.H.: ‘Accuracy improvement of low cost INS/GPS for land application’. Master’s thesis, Calgary University, 2001 35 Petovello, M.G.: ‘Real-Time integration of tactical grade IMU and GPS for high-accuracy positioning and navigation’. PhD thesis, University of Calgary, 2003 36 Lobo, J., Dias, J.: ‘Relative pose calibration between visual and inertial sensors’, Int. J. Robot. Res., 2007, 26, (6), pp. 561–575 37 Horn, B.K.P.: ‘Closed-form solution of absolute orientation using unit quaternions’, J. Opt. Soc. Am., 1987, 4, (4), pp. 629– 642 38 Hutton, J., Bourke, T., Scherzinger, B.: ‘New developments of inertial navigation systems at Applanix’, Report Applanix Corporation 39 Shin, E.H.: ‘Estimation techniques for low-cost inertial navigation’. PhD thesis, Calgary University, 2005

8 8.1

Appendix Coordinate frame

In strap down INS, coordinate frames are defined as follows: † e-frame: the Earth fixed coordinate frame used for position definition. It is defined with one axis parallel to the Earth’s polar axis and with the other axes fixed to the Earth and parallel to the equatorial plane. † l-frame (or N-frame): the local-level frame (or navigation frame), for which the Z-axis is parallel to the vertical at a position with respect to the local Earth surface. The axes of this l-frame correspond to WGS-84 east, north and up (ENU) on the Earth ellipsoid. † b-frame: the body frame with axes parallel to the righthanded orthogonal sensor-body axes. † c-frame: the camera frame with axes parallel to the righthanded orthogonal image sensor axes.

823

& The Institution of Engineering and Technology 2011