Using Doppler Radar and MEMS Gyro to Augment DGPS for Land Vehicle Navigation

Transcription

1 rd IEEE Multi-conference on Systems and Control, July 8-,9, Saint Petersburg, RUSSIA pp , (c)9 IEEE Using Doppler Radar and MEMS Gyro to Augment DGPS for Land Vehicle Navigation Jussi Parviainen, Manuel A. Vázquez López, Olli Pealin, Jani Hautamäi, Jussi Collin and Pavel Davidson Abstract This paper presents the development of a land vehicle navigation system that provides accurate and uninterrupted positioning. Ground speed Doppler radar and one MEMS gyroscope are used to augment differential GPS (DGPS) and provide accurate navigation during DGPS outages. Using Doppler radar has advantages of easy assembling and lowcost maintenance compared to wheel encoders. The Doppler radar and gyro are calibrated when DGPS is available. Loosely coupled Kalman filter gives optimally tuned navigation solution. Field tests were carried out to evaluate the performance of the system. The results show that position accuracy of.5 meters can be achieved during 5 seconds DGPS outages. I. INTRODUCTION Accurate navigation is a ey tas for automated ground vehicle control. Using differential GPS (DGPS) and real time inematic (RTK) satellite navigation, the position of a receiver can be determined with centimeter-level accuracy. However, there are some instances when GPS performance can be worse than expected. The satellite signals can be mased by buildings and other reflecting surfaces. DGPS performance degradation may also occur because of multipath. In order to overcome these difficulties some additional sensors that are not affected by the external disturbances can be used. The vehicle position during DGPS outages can be estimated using heading and velocity measurements. In our system ground speed from the Doppler radar and heading rate from the gyro are used to perform the dead reconing (DR) computations. In this paper we propose an integrated DGPS/DR system that provides accurate and uninterrupted navigation even during DGPS outages. Our main contribution is the calibration of Doppler radar which maes it suitable for accurate land vehicle navigation. There are different sensors such as accelerometers, wheel encoders, Doppler radars that give information about a vehicle translational motion. The conventional 6 degrees of freedom (DOF) inertial navigation system (INS) consists of three gyros and three accelerometers. The description of INS and its performance can be found in numerous wors, for example [], [], [], [4], [5]. The cost of INS depends significantly on the required navigation performance during DGPS outages. If we wanted to use INS for our application, we would need a tactical grade INS with the gyro of approximately deg/h accuracy. Using DR implementation Jussi Parviainen, Olli Pealin, Jani Hautamäi Jussi Collin and Pavel Davidson are with with Department of Computer Systems, Tampere University of Technology. firstname.lastname@tut.fi Manuel A. Vázquez López is with the Department of Mathematics, Tampere University of Technology. manuel.vazquez@tut.fi with one gyro and a ground speed sensor can significantly lower the cost of positioning system. The possible choice for ground speed sensor is a wheel encoder or Doppler radar. A wheel encoder provides information on the traveled curvilinear distance of a vehicle by measuring the number of full and fractional rotations of the vehicle wheels [6]. This is mainly done by an encoder that outputs an integer number of pulses for each revolution of the wheel. The number of pulses during a certain time period is then converted to the traveled distance through multiplication with a scale factor depending on the wheel radius. Many previous wors used wheel encoders to measure ground speed [7]. However, there are several sources of inaccuracy in the translation of the wheel encoder readings to traveled distance or velocity of the vehicle. They are [8], [9]: wheel slips, uneven road surfaces, sidding, and changes in wheel diameter due to variations in temperature, pressure, tread wear and speed. The first three error sources are terrain dependent and occur in a non-systematic way. This maes it difficult to predict and limit their detrimental effect on the accuracy of the estimated traveled distance and velocity. A non-contact speed sensor, such as a Doppler radar, can overcome these difficulties. It is much more robust against extreme environmental conditions such as mud and rain, its output is not affected by wheel slip and is easy to maintain. By combining the aforementioned gyroscope and Doppler radar, we have developed a low-cost DR system to accurately position a vehicle during short DGPS outages. Experimental results show that less than.5 meters position error can be achieved after 5 seconds of DGPS outage. A similar DR system was proposed in [], but no scale factor calibration was considered, neither for the gyroscope nor for the Doppler radar (the latter has a significant impact on the performance of our proposed system). Section II gives a brief overview of the Doppler radar, the gyroscope and the DGPS receiver used in our system. The algorithms are explained in detail in Section III. Our measurement setup is presented in IV-A. Finally, experimental results are shown in Section IV. II. INSTRUMENTATION A. Doppler Radar as Speed Sensor Conventionally, land vehicles ground speed is measured based on wheels rotation using wheel encoders. In these cases measured speed is sensitive to wheel slip and pressure of the tires. Also maintenance of wheel encoders can be difficult and expensive. Therefore Doppler radar was used to

2 measure the speed of the vehicle. In our tests we used Dicey John III radar []. The dynamic range of this radar is from m/h to 7 m/h. Output of the Doppler radar is always positive. Therefore the direction of vehicle movement cannot be determined based on Doppler radar output. In addition to this the Doppler radar is also insensitive to speed below m/h. Fig. illustrates the situation. Some studies have been carried out to detect the direction as in [], but this ind of radar is still not commonly used and is expensive. The output of Dicey John radar is a square wave whose frequency is proportional to the speed of the vehicle. The radar is attached to the vehicle at certain boresight angle, which is approximately 5 degrees. The measured Doppler shift frequency f d depends on speed as follows: f d = v(f /c)cos(θ), () where v,f,c and θ are the speed of vehicle, the transmitted frequency of radar, speed of light and inclination angle of radar. However, this angle θ can be slightly different from the nominal boresight angle and that affects the calculation of vehicle velocity. Thus the radar should be calibrated before use. The calibration includes the estimation of unnown scale factor (SF) error, which can be found using DGPS velocity. Once the radar is calibrated it can be used for accurate ground speed measurement. B. Gyroscope Analog Devices ADIS6 MEMS gyroscope was used for heading rate measurements. Output of the gyro is digital and can be read using serial peripheral interface (SPI) communication. According to sensor datasheet [] the gyro has bias stability of.6 /s (σ) and angle random wal is 6 / h (σ). The gyro was calibrated in laboratory for long term bias and scale factor. The calibrated values for bias and SF were found to be within specifications of data sheet. Bias stability was also estimated using Allan variance plot (Fig.). From Allan variance plot it can be seen, that bias stability is approximately. /s. Calibration was done using Velmex B599TS rotary table. Doppler radar speed - - Fig.. Measured speed (m/h) - Doppler speed versus real speed True speed (m/h) Allan deviation (deg/s) Allan deviation =.88 4 tau (s) Fig.. C. NovAtel DGPS receiver Allan variance of ADIS6 gyroscope In our tests, we used NovAtel DL-4+ dual frequency DGPS receiver. The GPS antenna was mounted on the roof of the car during tests. Accuracy of this receiver is tens of centimeters in differential GPS mode, which maes it suitable for reference position to evaluate the accuracy of dead reconing. It also provides synchronization pulses to Doppler radar and gyro. When good GPS signals are available the DGPS receiver is used to calibrate the Doppler radar and the gyro. III. NAVIGATION ALGORITHM The data from the sensors is processed using three different Kalman filters. One estimates the scale factor of the Doppler radar, another calibrates the gyro, and the Extended Kalman filter (EKF) computes the position and heading. Calibration of the Doppler radar and the gyro was performed by two different filters to eep the design robust, i.e. possible errors in other sensor do not affect to the calibration of both sensors. A. Doppler radar calibration Velocity obtained from the Doppler radar output at the time instant is modelled as v D = ( + S D ) v + n D () where v is the true (unnown) velocity at, S D is the scale factor error, and n D is a random variable (r.v.) of additive white gaussian noise (AWGN) whose mean and variance are nown. Scale factor error is considered constant and, hence, we can write S D = S. D () In order to estimate the scale factor error, horizontal velocity given by DGPS is used. This is assumed to be the true velocity distorted by AWGN, v DGPS with n DGPS N (,σ DGPS). = v + n DGPS, (4)

3 Using () and (4) we calculate the difference between Doppler radar and DGPS ground speed z D = v D v DGPS = S D v + n D n DGPS. (5) Equations () and (5) can be seen as the state equation and observation equation, respectively, of a dynamic system in state-space form. Since we are assuming that both n D and n DGPS are gaussian and independent, Kalman filter (KF) [4] can be applied to estimate the state, S D. The true velocity needed in (5) is not nown, though, and DGPS velocity will be used as an approximation in that equation. Once an estimate of the scale factor error is available, the true velocity is estimated using that obtained from the Doppler radar as ˆv = vd + ŜD (6) with ŜD being the final estimate of the Doppler radar scale factor error given by the KF. B. Gyroscope calibration Output of the gyroscope is heading rate and depends on two parameters, the bias and the scale factor error, that must be estimated every time the device is turned on. Typically, this is modeled as w g = ( + Sg )w + B g + n g (7) where w is the true heading rate of the vehicle at discrete time, S g is the scale factor error, B g is the bias, and n g is additive white gaussian noise, whose variance is nown to be σg. The reference signal used to calibrate the gyroscope, i.e., to estimate the values of parameters B g and Sg, is the heading given by DGPS, Ψ DGPS. It is affected by noise so that Ψ DGPS = Ψ + n DGPS, (8) being Ψ the true heading of the vehicle at time, and n DGPS a r.v. of white gaussian noise whose variance, σdgps (vdgps ), depends on velocity. If the initial attitude, Ψ, is nown, the current heading can be obtained from the gyroscope by integrating the heading rate, which in discrete-time amounts to Ψ g = w g i t + Ψ, (9) i= with t representing the sampling period or time interval between two consecutive gyro measurements. Substituting (7) in (9) yields Ψ g = ( ( + S g )w i + B g + n g ) i t + Ψ = i= i= + w i t + i= i= n g i t + Ψ = Ψ + S g i= S g w i t + i= B g t+ w i t + B g t + where it has been taen into account that i= n g i t, () Ψ = Ψ + w i t. () i= Subtracting equation () from (8) we compute the difference between DGPS heading (our reference signal) and that obtained from the gyroscope, z g = ΨDGPS i= Ψ g = Bg t S g w i t + n DGPS i= n g i t, () which can be seen as the observation equation of a dynamic system whose state comprises the bias and scale factor error. These are assumed constant, and thus the equations that govern the evolution of the system are simply: B g = Bg () S g = Sg. (4) Equation () (observation equation) involves the true value of the variable we want to estimate, i.e., the true heading rate, w i, in this case. This is solved by using the heading rate from gyroscope, w g i, as an approximation, in which case we have z g = Bg t Sg i= w g i t + ndgps i= n g i t, (5) where it has also been used that B g = Bg and S g = Sg. The term i= ng i t in (5) is a random wal driven by the gyroscope noise, and hence is a zero mean gaussian r.v. [5] whose variance (which grows linearly over time) can be computed from the gyroscope manufacturer specifications (angle random wal). However, the corresponding stochastic process is not white (its value at time highly depends on its value at time ), and hence the KF cannot be applied to estimate the gyroscope bias and scale factor in the dynamic system given by equations (), (4) and (5). In order to overcome this difficulty, the term i= ng i t, which will be denoted as δω RW, is going to be modelled as an additional state that needs to be estimated. It evolves according to the equation δω RW = δω RW + n δωrw (6)

4 where n RW = n g t is a r.v. of AWGN whose variance is σg t. Using (6) the observation equation can be rewritten as z g = Bg t Sg w g i t δωrw + n DGPS, (7) i= and this along with equations (), (4) and (6) define a linear-gaussian dynamic system in state-space form that allows for the KF to be applied in order to estimate the bias and scale factor error. From those, and using equations () and (), the latter being rewritten as S g Ψ = S g Ψ + S g w i t, (8) i= the compensated heading obtained from the gyroscope output is i= ˆΨ = Ψ + wg i t ˆB g t ) (9) ( + Ŝg being ˆB g and Ŝg the final estimates of the gyroscope bias and scale factor error, respectively, computed by the KF after the calibration process. C. Position and heading estimation Vehicle dead reconing computations can be described by the following equations P N = v cos(ψ) P E = v sin(ψ) () Ψ = w, where P N and P E are the north and east components of vehicle position, Ψ is heading, v is ground speed (measured by DGPS when it is available and Doppler radar during DGPS outages) and w is gyro heading rate measurement. The EKF is used to solve this non-linear estimation problem. The augmented state vector for the EKF is x = [ P N P E Ψ δv D δω g ] T, () where δω g and δv D are added to the EKF as additional states in order to compensate for the residual non-white gyro and Doppler radar errors, respectively, that remain after the sensors are calibrated according to sections III-A and III-B. These two states are modeled as first order Gauss Marov process with time constants τ g and τ D. The covariance propagation in the EKF is P + = Φ P Φ T + Q, () where Φ is the discrete equivalent of the continuous transition matrix F and Q is the process noise matrix. In our case we have the following transition matrix F = v D sin(ψ) cos(ψ) v D cos(ψ) sin(ψ) /τ D /τ g. () Fig.. Doppler radar is attached at rear and GPS antenna is on the roof of the car, gyro was mounted inside The measurement equation is where H = and z = [ P DGPS N z = Hx + η, (4) P DGPS E (5) Ψ ] DGPS T (DGPS north position, east position and course over ground (heading) measurement, respectively) and η is zero mean white noise with covariance matrix R. Since DGPS computes heading using arctangent of north and east velocity components, the standard deviation of heading is inversely proportional to the ground speed. Therefore, accuracy of the heading measurement Ψ DGPS degrades as speed decreases [6], i.e., σ Ψ DGP S = σ v DGP S (6) v where σ Ψ DGP S and σ v DGP S are the standard deviations of DGPS heading and velocity errors. A. Measurement setup IV. EXPERIMENTAL RESULTS In order to assess the performance of the proposed DR system, several experiments were done. Measurements were taen using normal passenger car with Dicey-John III Doppler radar attached to the rear and DGPS antenna placed on the top of the roof as illustrated in Fig.. ADIS6 gyro was securely fixed inside the vehicle. The gyro and Doppler radar were connected to National Instruments NI USB-845 and NI USB-655 devices. Gyro was found out to be sensitive to fluctuations of the supply voltage, and therefore a voltage regulating circuit with 5 V LM94T-5. LDO regulator was used. Also, the analog bandwidth of the gyro was reduced from Hz to 5 Hz using an external nf capacitor. Gyro communicated via serial peripheral interface (SPI) bus, and Doppler radar used symmetrical square wave.

5 Position data and timestamps Timestamping pulse Novatel DGPS Fig. 4. National Instruments USB-845 Laptop PC (post processing) SPI bus Gyroscope USB bus The measurement setup flowchart National Instruments USB-655 Ground speed Doppler radar Pulse GPS speed Radar speed (m/s) Speed (m/s) 5 4 Insensitivity zone Doppler radar speed GPS speed time (s) Insensitivity zone time (s) Fig. 5. Doppler radar speed versus DGPS speed and their difference in trajectory. The GPS receiver provided an edge-triggered timestamping mechanism. The SPI communication with gyro was handled by NI USB-845 device, which acted as the bus master. The device had also digital output, which was used to create timestamp pulses for the GPS receiver. The frequency of the square wave was measured using the capability of the NI USB-655 to count edges from an input signal. The timestamping mechanism of the GPS receiver has, however, a frequency upper limit of Hz, which is less than the system s sampling rate of Hz. Therefore, the final synchronization of all gyro and edge counter measurements to GPS system time was done in post-process phase by interpolation. The flowchart of the measurement setup is illustrated in Fig. 4. B. Measurement Results Our test vehicle was driven along several trajectories in a paring lot while data delivered by the gyroscope and the Doppler radar were recorded. DGPS was available all the time and provided estimates of the vehicle position that served two purposes: at first to calibrate both the Doppler radar and the gyroscope (see Sections III-A and III-B, respectively), and then to evaluate the accuracy of our DR system. In Fig. 5 the differences between Hz output of DGPS ground speed and Doppler radar speed are illustrated after the calibration was done. When the vehicle starts moving, large discrepancy between Doppler radar and DGPS speed is explained by the insensitivity zone of the Doppler radar to the speed below m/h. Once speed is increased above this threshold, Doppler error becomes smaller. From the upper plot of Fig. 5 we can see that Hz output of Doppler is much more noisy compared to DGPS. The standard deviation of Doppler radar ground speed error is approximately. m/s. Examples of the test trajectories are presented in Figs. 6 and 7. The blac dashed lines on these plots depict a calculated DR trajectory during 5 second navigation without DGPS DR outage start Fig. 6. Trajectory (solid line) driven with test vehicle. The dashed line illustrates an example where dead reconing was used for 5 seconds. using DGPS data. The tests showed that we are able to estimate horizontal position with the accuracy better than.5 meters in 99 percent of all tests. The position error was calculated as the difference between DR and DGPS positions. Since the outages are simulated the original DGPS data exist also during simulated outages and the errors caused by DR can be calculated. Figs. 8 and 9 present the histograms of maximum alongtrac and cross-trac errors during 5 seconds DGPS outage. This data is based on 7 realizations. Fig. presents the histogram of maximum along-trac and cross-trac errors during seconds DGPS outage. We can see that a reasonable accuracy is also achieved during seconds outage. V. CONCLUSION In the paper we showed that Doppler radar with MEMS gyro can be used as DR system to aid DGPS during signal outages. This system is easy to mount and has low cost compared to wheel encoder systems. Kalman filters were m

6 .5 cross trac along trac outage start.5 error (m) DGPS DR m % Fig. 9. Histogram of maximum along-trac and cross-trac errors from 7 DR tests of trajectory with 5 seconds DGPS outage. Fig. 7. Trajectory (solid line) driven with test vehicle. The blac dashed line illustrates an example where dead reconing was used for 5 seconds cross trac along trac.5.5 error (m) cross trac along trac.5 error (m) % % Fig. 8. Histogram of maximum along-trac and cross-trac errors from 7 DR tests of trajectory with 5 seconds DGPS outage. implemented to estimate scale factor error for Doppler radar speed sensor, and scale factor and bias for gyroscope. Also, an extended Kalman filter was used to estimate initial heading and position for DR system. The measurement results showed that that during 5 second outage, less than.5 meter cross and along trac errors could be achieved. VI. ACKNOWLEDGMENTS The wor described in this paper was carried out in the project Future GNSS Applications and Techniques (FUGAT) funded by the Finnish Funding Agency for Technology and Innovation (Tees). REFERENCES [] J. Farrell and M. Barth, The Global Positioning System and Inertial Navigation, rd ed. McGraw-Hill, 999. [] D. Titterton and J. Weston, Strapdown Inertial Navigation Technology, nd ed. IEE, 4. [] P. D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House Publishers, 8. [4] P. Savage, Strapdown inertial navigation integration algorithm design, part : attitude algorithms, Journal of Guidance, Control and Dynamics, vol., pp. 9 8, January 998. [5], Strapdown inertial navigation integration algorithm design, part : velocity and position algorithms, Journal of Guidance, Control and Dynamics, vol., pp. 8, March 998. Fig.. Histogram of maximum along-trac and cross-trac errors from 7 DR tests of trajectory with seconds DGPS outage. [6] E. Abbott and D. Powell, Land-vehicle navigation using GPS, Proceedings of the IEEE, vol. 87, pp. 45 6, Januaury 999. [7] H. Chung, L. Ojeda, and J. Borenstein, Accurate mobile robot deadreconing with a precision-calibrated fiber-optic gyroscope, IEEE Transactions On Robotics And Automation, vol. 7, no., February. [8] J. Borenstein and L. Feng, Measurement and correction of systematic odometry errors in mobile robots, IEEE Trans. Robot. Automat, vol., pp , December 996. [9] R. Carlson, J. Gerdes, and J. Powell, Error sources when land vehicle dead reconing with differential wheelspeeds, the Journal of The Institute of Navigation, vol. 5, no., pp. 7, December 4. [] D. M. Bevly and B. Parinson, Cascaded Kalman filters for accurate estimation of multiple biases, dead-reconing navigation, and full state feedbac control of ground vehicles, Control Systems Technology, IEEE Transactions on, vol. 5, no., pp. 99 8, March 7. [] Dicey-John Radar III datasheet. [Online]. Available: www. dicey-john.com/ media/7--7.pdf [] R. Rasshofer and E. Biebl, A direction sensitive, integrated, low cost Doppler radar sensor for automotive applications, Microwave Symposium Digest, 998 IEEE MTT-S International, vol., pp vol., June 998. [] Analog Devices ADIS6 Data sheet. [Online]. Available: http: // sheets/adis6.pdf [4] R. E. Kalman, A new approach to linear filtering and prediction problems, Transactions of the ASME Journal of Basic Engineering, pp. 5 45, 96. [5] W. Stocwell, Angle random wal,. [Online]. Available: www. xbow.com/support/support pdf files/anglerandomwalappnote.pdf [6] D. M. Bevly, GPS: A low cost velocity sensor for correcting inertial sensor errors on ground vehicles, Journal of Dynamic Systems, Measurement, and Control, vol. 6, no., pp , June 4.