Doppler Radar and MEMS Gyro Augmented DGPS for Large Vehicle Navigation
Jussi Parviainen∗, Martti Kirkko-Jaakkola∗, Pavel Davidson∗, Manuel A. V´azquez L´opez†, and Jussi Collin∗
∗ Department of Computer Systems
Tampere University of Technology, Tampere, Finland Email: firstname.lastname@tut.fi
† Department of Signal and Communications Theory Carlos III University, Madrid, Spain
Email: mvazquez@tsc.uc3m.es
Abstract— This paper presents the development of a land vehi- cle navigation system that provides accurate and uninterrupted positioning. A ground speed Doppler radar and one MEMS gyroscope are used to augment differential GPS (DGPS) and provide accurate navigation during GPS outages. The goal is to maintain a position accuracy of2meters or better for15seconds when an accurate GPS solution is not available. The Doppler radar and gyro are calibrated when DGPS is available, and a loosely coupled Kalman filter gives an optimally tuned navigation solution. Field tests were carried out in a harbor environment using straddle carriers.
I. INTRODUCTION
Accurate navigation is a key task for automated ground vehicle control. Using code based differential GPS (DGPS) and real time kinematic (RTK) DGPS satellite navigation, the position of a receiver can be determined with sub meter or even with centimeter-level accuracy. However, there are some instances when GPS performance can be worse than expected. The satellite signals can be masked by buildings and other reflecting surfaces. GPS 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. During GPS out- ages or unreliable position fixes, the vehicle position 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 reckoning (DR) computations.
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 inertial navigation system (INS) consists of three gyros and three accelerometers. The description of INS and its perfor- mance can be found in numerous works, for example [1]–
[5]. The cost of INS depends significantly on the required navigation performance during GPS outages. If we wanted to use INS for our application, we would need a tactical grade INS with the gyro of approximately10deg/h accuracy. A DR implementation using a single 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 measures the
distance traveled by a vehicle by counting the number of full and fractional rotations of a wheel [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 works 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, skidding, 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 makes 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. Its output is not affected by wheel slip and the device is easy to maintain. However, the Doppler radar is not completely independent of environmental conditions; for instance, depending on the placement of the radar, splashing water may cause errors in the system.
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. Previously, a DR system including Doppler radar and gyro was proposed in [10] for agricultural devices. The previous work of au- thors containing same sensors, gyro and Doppler radar, is presented in [11]. The basic navigation algorithm is quite similar compared to our previous work. However, there are some differences when the similar sensors are applied to large land vehicles. In this paper those differences are introduced.
In addition, we show the results of actual straddle carrier tests in a harbor area using our integrated DGPS/DR system that provides accurate and uninterrupted navigation even during DGPS outages.
Paper continues with Section II giving 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. Finally, experimental results with straddle carriers are shown in Section IV.
II. INSTRUMENTATION
A. Doppler Radar as Speed Sensor
Conventionally, the ground speed of a land vehicle is mea- sured based on wheels rotation using wheel encoders. In these cases measured speed is sensitive to wheel slip and pressure of the tires. Moreover, maintenance of wheel encoders can be difficult and expensive. Therefore Doppler radar was used to measure the speed of the vehicle. In our tests we used Dickey John III radar [12]. The dynamic range of this radar is from 0.5 km/h to107km/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 0.5km/h. Fig. 1 illustrates the insensitivity zone. Some studies have been carried out to detect the direction as in [13], but this kind of radar is still not commonly used and is expensive.
The output of Dickey 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 35 degrees. The measured Doppler shift frequencyfd depends on speed as follows:
fd= 2v(f0/c) cos(θ), (1) wherev, f0, candθ 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 unknown scale factor (SF) error, which can be found using GPS velocity. Once the radar is calibrated it can be used for accurate ground speed measurement.
B. Gyroscope
Analog Devices ADIS16130 MEMS gyroscope was used for heading rate measurements. Output of the gyro is digital and can be read using serial peripheral interface (SPI) com- munication. According to sensor datasheet [14] the gyro has bias stability of 0.0016◦/s (1σ) and angle random walk is 0.56◦/√
h (1σ).
0 3
-3
True speed (km/h) Measured speed (km/h)
0.5
0.5
-0.5 3
-3 Doppler radar speed
Fig. 1. Doppler radar speed measurement
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.
C. DGPS receiver
In the tests, we used dual frequency Javad receiver. The GPS antenna was mounted at the top of the vehicle close to the center of rotation. The accuracy of this receiver is about ten centimeters in the real time kinematic (RTK) DGPS mode, which makes it suitable for reference position to evaluate the accuracy of dead reckoning. The receiver can operate in four different modes: standalone, code DGPS, RTK float solution, and RTK fixed solution. Naturally, the RTK fixed solution mode is the most accurate. Whenever such a solution is available, the DGPS receiver is used to calibrate the Doppler radar and the gyro.
III. NAVIGATION ALGORITHM
Like in our previous work [11], the data obtained 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 keep the design robust, i.e. possible errors in other sensor do not affect to the calibration of both sensors.
A. Doppler radar calibration
Because Doppler radar and GPS antenna are located in different places of the vehicle, a lever arm compensation is needed. The GPS antenna is located almost at the center of rotation of the vehicle and the distance between the antenna and the radar is known. In absence of other errors, the lever arm correction is computed as follows:
vD=vDGP S+w×R (2) whereR is the vector pointing from the GPS antenna to the Doppler radar; w is the heading rate measurement (rad/s);
vD and vDGP S are the speeds of the Doppler radar and GPS antenna, respectively.
However, before we can compensate for the lever arm, we have to calibrate the scale factor error of the Doppler radar.
This can be done while the vehicle is driving along a straight line. The velocity measured by the Doppler radar at timekis modeled as
vkD= 1 +SD
vk+nDk (3) wherevk is the true (unknown) velocity at timek,SDis the scale factor error, andnDk is a random variable (r.v.) of additive white Gaussian noise (AWGN) whose mean and variance are known. The SF error is modeled as a random constant. In order to estimate it, the horizontal velocity given by GPS is used.
This is assumed to be the true velocity distorted by AWGN, vkDGP S=vk+nDGP Sk , (4) withnDGP S∼ N 0, σDGP S2
.
Using (3) and (4) we calculate the difference between Doppler radar and GPS ground speed
zDk =vkD−vkDGP S=SDvk+nDk −nDGP Sk . (5) Equation (5) can be seen as the observation equation of a dy- namic system in state-space form. Since we are assuming that bothnDk andnDGP Sk are Gaussian and independent, Kalman filter (KF) [15] can be applied to estimate the state SD. The true velocity needed in (5) is not known, though, and GPS 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
ˆ
vk= vDk
1 + ˆSD (6)
with SˆD being the final estimate of the Doppler radar scale factor error given by the KF.
B. Gyroscope calibration
The output of the gyroscope at time kcan be modeled as wgk = (1 +Sg)wk+Bg+ngk
=wk+Sgwk+Bg+ngk
(7) where wk is the angular rate at epoch k, Sg is the gyro SF error,Bg is the bias and ngk denotes random noise which we model as AWGN with variance σ2g. Modern MEMS gyros have quite good bias stability, especially when the temperature is constant. However, the day-to-day bias can be significant.
Therefore, initial bias calibration has to be performed before starting navigation. The bias may also fluctuate during oper- ation; it is particularly sensitive to the ambient temperature.
Consequently, when navigating for longer periods, the gyro- scope bias should be recalibrated whenever possible.
The SF error is caused by gyro misalignment and sensitivity error. If the vehicle is not doing significantly more left turns than right turns, or vice versa, the SF error will be canceled by opposite turns; in the extreme case of driving circles in a single direction, even a moderate SF error may accumulate into dozens of degrees of heading error. As we assume that these extreme cases do not occur, the position error accumulation because of SF is insignificant. Therefore, we omit the SF error compensation.
When the vehicle is stationary, the angular ratewis known to be zero and thus, the gyro output consists solely of the bias Bg and noise ng. Therefore, when the vehicle is started, we require it to be stationary for a certain period of time during which the gyro bias is estimated. In this initial calibration, we compute the gyro bias simply by averaging. In real-time implementations this can be done recursively as
Bˆg1=w1
Bˆkg =k−1
k Bˆkg−1+wk
k . (8)
Since the gyroscope noise variance σ2g is known from the technical specifications and laboratory tests, the initial gyro
calibration can be done by means of Kalman filtering instead of recursive averaging. Obviously, if there is prior knowledge available on the bias, Kalman filtering may converge faster and be more accurate.
Whenever the vehicle stops for a longer time, the gyro bias estimate can be refined. First of all, since we know that the bias changes with time, the accuracy of the bias estimate degrades as time passes; moreover, even if there is only a short time since the initial calibration, having more data for averaging should improve the estimate. For later updates of the gyro bias estimate, we use a KF because it allows the estimate to adapt to changes in the bias by gradually increasing its variance estimate. If this was to be done using recursive averaging, the weights in (8) should be modified to give more weight to the current measurement.
For calibrating the gyro bias during navigation, it has to be known whether the vehicle is stationary or not. The Doppler radar is insensitive to speeds lower than 0.5km/h; therefore, additional sensors, e.g. accelerometers, are required for stop detection. Furthermore, it may be beneficial to ensure that the stop is sufficiently long in order to make sure there are enough samples for averaging. If there is any uncertainty in the stop detection, a certain number of first and last samples may be discarded at each stop.
C. Position and heading estimation
Vehicle dead reckoning computations can be described by the following equations
P˙N = vcos(Ψ) P˙E = vsin(Ψ)
Ψ˙ = w,
(9)
where PN and PE are the north and east components of vehicle position,Ψis heading, (˙) indicates the time derivative operation, v is the ground speed (measured by GPS when available and by Doppler radar otherwise) andw is the gyro heading rate measurement. It should be noted that even if we were using ideal sensors, the vehicle body heading produced by the gyro and the ground track direction measured by GPS may not be exactly the same during turns. In this work, we approximate that heading measured by the gyro is same as GPS based heading.
The EKF is used to solve this non-linear estimation problem.
The augmented state vector for the EKF is x=
PN PE Ψ δvD δωg T
, (10) whereδωg andδvD 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 Markov process with time constantsτg andτD.
The covariance propagation in the EKF is
Pk+1|k = ΦkPk|kΦTk +Qk, (11)
whereΦis the discrete equivalent of the continuous transition matrix F andQ is the process noise matrix. In our case we have the following transition matrix
F =
0 0 −vDsin(Ψ) cos(Ψ) 0 0 0 vDcos(Ψ) sin(Ψ) 0
0 0 0 0 1
0 0 0 −1/τD 0
0 0 0 0 −1/τg
, (12)
where heading Ψcan be calculated using previous output of the filter and gyro.
The measurement equation is
z=Hx+η, (13)
where
H =
1 0 0 0 0 0 1 0 0 0 0 0 1 0 0
(14) andz=
PNDGP S PEDGP S ΨDGP S T
(GPS north posi- tion, east position and course over ground (heading) measure- ment, respectively) andηis zero mean white noise with covari- ance matrixR. Since GPS 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 ΨDGP S degrades as speed decreases [16], i.e.,
σΨDGP S =σvDGP S
v (15)
Doppler radar GPS antenna
Gyro
Fig. 2. Measurement device
0 50 100 150 200 250
−0.8
−0.6
−0.4
−0.2 0 0.2 0.4 0.6 0.8
time [s]
speed error [m/s]
DGPSspeed − Doppler speed w/o cor DGPSspeed − Dopplerspeed w/ cor
Fig. 3. Speed error due to lever arm
where σΨDGP S and σvDGP S are the standard deviations of GPS heading and velocity errors. Also position error in GPS receiver varies depending on which mode it is operating (RTK or code DGPS). Thus this must be also taken into account when forming covariance matrixR.
IV. EXPERIMENTAL RESULTS
As discussed in the previous sections, a similar system was used in [11] where the test vehicle was a regular passenger car. In this section we present the main problems we encounter when a 13-meter-tall straddle carrier (Fig. 2) is used instead.
The gyro was located at the top of the vehicle and the Doppler radar was mounted between the right wheels. Thus, as discussed in Section III-A, a lever arm error will occur. The speed difference due to the lever arm is shown in Fig. 3.
Although the Doppler radar is insensitive to wheel slippage and to other wheel based errors, new disturbances were encountered during straddle carrier tests. As the radar was mounted between the tires, water and mud may splash to the radar beam, causing errors in the velocity measurement. This error can be reduced only by changing the location of the radar. An example of a test made on a wet road is shown in Fig. 4. It can be seen that the largest errors occur during high speeds.
Another difference in the behavior of large vehicles com- pared to passenger cars is the vehicle swing as illustrated in Fig. 5. Usually, this can be observed as an oscillation in the GPS heading measurement, as in Fig. 6. In order to compensate for this error we need to increase the variance of GPS heading measurement error in the EKF or model the oscillations directly in the navigation algorithm. However, as the swinging does not occur all the time, it is easier to just increase the heading error variance in Kalman filter.
In a real harbor environment there are obstacles which obstruct the view to GPS satellites. Occasionally, an in- sufficient number of visible satellites prevents the receiver from computing a fixed RTK solution, forcing it to operate in an inferior mode. However this can be compensated by
0 50 100 150 0
2 4 6 8
time [s]
speed [m/s]
DGPSspeed Doppler
speed
0 50 100 150
−1 0 1 2 3
time [s]
speed error [m/s]
DGPSspeed − Dopplerspeed
Fig. 4. Splashing water causes errors in the Doppler radar speed measurement
using different covariances in navigation filter for the different modes.
Our test vehicle was driven along several trajectories in a harbor area while data delivered by the gyroscope and the Doppler radar was recorded. A sample route is presented in Fig. 7. The red curve presents GPS trajectory and the black dashed line is the DR trajectory when GPS data was not used.
Several 15-second GPS outages were generated in order to evaluate the horizontal position accuracy of the DR solution.
Fig. 8 presents the distribution of horizontal position errors of the trajectory shown in Fig. 7 with multiple artificial 15- second outages made into the GPS data. The figure shows that during this test, in approximately 80 % of the outages, the maximum position error did not exceed2meters. Mean square error in this case is approximately 1.9 meters. In general, compared to the passenger car results [11], the navigation accuracy during DGPS outages degraded slightly. Most likely,
Fig. 5. Vehicle swing
128 130 132 134 136 138 140 142
0 10 20 30 40 50 60
Time (s)
Heading (deg)
DGPS Gyro
Fig. 6. GPS heading error due to the vehicle swing
the largest errors in the data were caused by the splashing water which induced significant non-Gaussian errors to the Doppler radar measurements. Fortunately, large errors were fairly rare and in the most of cases after 15 second GPS outage the position error was less than 2 meters. By using other types of sensors e.g. wheel encoders would arguably improve the results, as it is shown in [17]. However, the use of wheel encoders has other drawbacks which were discussed in the section I.
V. CONCLUSION
In this paper we showed that a Doppler radar and a MEMS gyro can be used as an accurate DR system to aid differential GPS during signal outages for straddle carriers and other harbor vehicles. If the vehicle does not have a standard speed
Outage ends
Outage starts
20 m
Fig. 7. Example route with 15 second GPS outage
0 20 40 60 80 100 0
1 2 3 4 5
Horizontal position error [m]
%
Fig. 8. Distribution of maximum horizontal position errors during multiple 15 second GPS outages
sensor or if the wheels are expected to slip considerably, a Doppler radar is advantageous compared to wheel encoders.
This paper shows that there are many aspects that are needed to take into account when designing this kind of navigation system for large vehicles. The mounting place of Doppler radar is crucial because a significant part of large position errors can be related to increased speed measurement errors on wet surfaces. For example, water splashing from the tires can cause more than1 m/s of speed error. In addition, during turns, the dynamics of an all-wheel-steered straddle carrier is in general very different from that of front-wheel-steered passenger cars.
Our test vehicle, a straddle carrier, was driven along various trajectories in a harbor area while the heading rate and speed measured by the gyroscope and the Doppler radar were recorded. Separate Kalman filters were used to estimate the scale factor error of the Doppler radar, the gyroscope bias, and the vehicle position and heading. The test results also showed that during short 15 second outage, an accuracy better than 2 meters is usually attained. Currently, the performance goals are not fully achieved, but improvements are to be made in the future. The current implementation is for post- processing real-world data, but the algorithm can be adapted for implementation as a real-time system.
REFERENCES
[1] J. Farrell and M. Barth, The Global Positioning System and Inertial Navigation, 3rd ed. McGraw-Hill, 1999.
[2] D. Titterton and J. Weston,Strapdown Inertial Navigation Technology, 2nd ed. IEE, 2004.
[3] P. D. Groves,Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House Publishers, 2008.
[4] P. Savage, “Strapdown inertial navigation integration algorithm design, part 1: attitude algorithms,”Journal of Guidance, Control and Dynamics, vol. 21, pp. 19–28, January 1998.
[5] ——, “Strapdown inertial navigation integration algorithm design, part 2: velocity and position algorithms,”Journal of Guidance, Control and Dynamics, vol. 21, pp. 208–221, March 1998.
[6] E. Abbott and D. Powell, “Land-vehicle navigation using GPS,”Pro- ceedings of the IEEE, vol. 87, pp. 145–162, Januaury 1999.
[7] H. Chung, L. Ojeda, and J. Borenstein, “Accurate mobile robot dead- reckoning with a precision-calibrated fiber-optic gyroscope,” IEEE Transactions On Robotics And Automation, vol. 17, no. 1, February 2001.
[8] J. Borenstein and L. Feng, “Measurement and correction of systematic odometry errors in mobile robots,”IEEE Trans. Robot. Automat, vol. 12, pp. 869–880, December 1996.
[9] R. Carlson, J. Gerdes, and J. Powell, “Error sources when land vehicle dead reckoning with differential wheelspeeds,” the Journal of The Institute of Navigation, vol. 51, no. 1, pp. 12–27, December 2004.
[10] D. M. Bevly and B. Parkinson, “Cascaded Kalman filters for accurate estimation of multiple biases, dead-reckoning navigation, and full state feedback control of ground vehicles,”Control Systems Technology, IEEE Transactions on, vol. 15, no. 2, pp. 199–208, March 2007.
[11] J. Parviainen, M. Lopez, O. Pekkalin, J. Hautam¨aki, J. Collin, and P. Davidson, “Using Doppler radar and MEMS gyro to augment DGPS for land vehicle navigation,” inProc. of 3rd IEEE Multi-conference on Systems and Control, july 2009, pp. 1690 –1695.
[12] Dickey-John Radar III datasheet. [Online]. Available: www.dickey-john.
com/ media/11071-0313-200702 1.pdf
[13] R. Rasshofer and E. Biebl, “A direction sensitive, integrated, low cost Doppler radar sensor for automotive applications,” Microwave Symposium Digest, 1998 IEEE MTT-S International, vol. 2, pp. 1055–
1058 vol.2, June 1998.
[14] Analog Devices ADIS16130 Data sheet. [Online]. Available: http:
//www.analog.com/static/imported-files/data sheets/ADIS16130.pdf [15] R. E. Kalman, “A new approach to linear filtering and prediction
problems,” Transactions of the ASME Journal of Basic Engineering, pp. 35–45, 1960.
[16] D. M. Bevly, “GPS: A low cost velocity sensor for correcting inertial sensor errors on ground vehicles,” Journal of Dynamic Systems, Mea- surement, and Control, vol. 126, no. 2, pp. 255–264, June 2004.
[17] J. Gao, M. Petovello, and M. Cannon, “Development of precise GPS/INS/wheel speed sensor/yaw rate sensor integrated vehicular po- sitioning system,” in Proc. of the 2006 National Technical Meeting of The Institute of Navigation, January 2006, pp. 780–792.
