2001 IEEE International Conference on Robotics and Automation, Seoul, Korea, May 21-26, pp. 3588-3593.
Sensor fusion for Mobile Robot Dead-reckoning With a Precision-calibrated Fiber Optic Gyroscope By Hakyoung Chung , Lauro Ojeda**, and Johann Borenstein** *
*
Seoul National University of Technology, Department of Control and Instrumentation Engineering 172 Kongneung-dong, Nowon-gu, Seoul 139-743, Korea,
[email protected] ** The University of Michigan, Advanced Technologies Lab 1101 Beal Ave., Ann Arbor, MI 48109-2110, USA.
[email protected],
[email protected].
ABSTRACT
to run a mission of, say, 10 minutes duration, while incurring an orientation error of at most 1.2°. However, with the reduction of bias drift errors other measurement errors gain in relative significance. Most notable among these hitherto secondary error are (1) the nonlinearity of the scale factor and (2) the susceptibility of the sensor to changes in ambient temperature. These two errors can be reduced substantially by performing an individual calibration for each gyro[3,4]. When more than one sensor modality is used to measure one parameter, such as relative orientation using an encoder and a gyroscope, then the question arises how to fuse the readings from both. In [5], Barshan and Durrant-Whyte proposed a direc Kalman filter that estimates a robot position and attitude. Other researches [6, 7] utilize gyroscopes and encoders to obtain a close estimate of the robot position. However, these methods need fast computations due to the rapidly varying nature of the robot dynamics. However, these methods need fast computations due to the rapidly varying nature of the robot dynamics. We propose an indirect Kalman filter that estimates error states and does not require fast computation[8].
1
Recently, fiber optic gyros with very low drift rates have become available and affordable. Because of their low drift rate attention is warranted to sources of errors that were previously considered as of secondary importance. In the KVH E-Core RD2100 gyros, we found that the temperature dependency and the non-linearity of the scale-factor caused significant errors. A precision calibration procedure, described in this paper, reduces the resulting errors by one order of magnitude. In addition to the calibration procedure, we integrated the gyro with the odometry system of a four-wheel drive, skid-steer Pioneer AT mobile robot by means of an indirect feedback Kalman filter that fuses the sensor data from both sensor modalities. Based on our experimental results this paper compares the relative effectiveness of both enhancements (precision gyro calibration and Kalman filter).
1.
INTRODUCTION
One key concern with the use of gyros as a means for improving dead-reckoning in mobile robots is their inherent bias drift. Until just a few years ago all but the highest-grade mechanical gyroscopes had large bias drift rates. However, this situation changed with the reduction in cost of commercially available fiber optic gyros. For example, the KVH E-Core RD2100 gyro [1,2], which was the model used in our experiments, costs about $2,000 and its drift rate is specified as (at most) 0.002°/s = 0.12°/min = 7.2°/hr (see Table I for specifications, as published by the manufacturer). With these specifications it is feasible
Table I: Technical specifications for the E-Core RD2100 fiber optic gyroscope. (Courtesy of [KVH])
RD2100
Performance Input Rate (max) Resolution Scale factor Nonlinearity Full Te mp Bias Stability Constant Temp Full Temp Repeatability
1
The full version of this paper was submitted for publication in the IEEE Transaction on Robotics and Automation, February 2000.
Angle Rand. Walk
Bandwidth
The part of this paper that deals with the gyroscope calibration was presented at the 2000 IEEE Int. Conf. on Robotics and Automation, San Francisco, CA, April 24-28, 2000, and is repeated here for convenience of the reader.
1
± °/sec °/sec °/bit %, rms %, p-p
100 0.004 0.000305 0.5 2
°/sec, 1 °/sec, p-p °/sec, p-p (°/hr)/ Hz °/ hr 3 dB, 45° phase shift
0.002 0.2 0.002 5 0.08 100
( )
2 3 ε ω g = a 0 + a1ω g + a 2ω g + a3ω g .
This paper presents a calibration procedure developed specifically for the KVH E-Core RD2100 and careful sensor error modeling for an indirect Kalman filter. As will be seen, consistent improvements of one order of magnitude in accuracy over a wide range of operating conditions are achieved by applying our calibration procedure. The fiber optic gyroscope, calibrated in the way described in Section 2, is used in a dead-reckoning system on our Pioneer AT mobile robot, and the system is used in indoor and outdoor environments. The implementation of our Kalman filter is detailed in Section 3, while Section 4 provides experimental results.
2.
2.2
Effect of the changes in temperature
Figure 2 shows how the measur ement error ε varies with temperature. This relationship can be expressed in terms of a second-order polynomial (3) ε (t ) = b0 + b1T + b2 T 2 . Parameters bi are not constant but depend of the input rate of rotation, ω. This means that all experiments must be repeated at different temperatures to calculate the final error model or compensation function of the gyro.
GYROSCOPE CALIBRATION
2.3
In this Section we briefly explain how to build a mathematical model for representing the error of the gyro due to the non-linearity in the scale factor and changes in temperature. The readers can find more details in [3]. For compensating static bias drift error, which should be treated as the most important eve n though mentioned briefly in this paper, common compensation procedure is utilized. That is to collect several samples (for, say, 5-10 seconds) with the gyro stationary and subtract the mean of the samples from subsequent measurements.
2.1
(2)
Building the error function
In order to determine the calibration function for the gyro, gyro data was collected under varying conditions. We will refer to the gyro data collected during rotation at a fixed rate of rotation and at a fixed temperature as one data set. In practice this is done by defining two data vectors for each rate of rotation, ωi. One for the output of the gyro, ωgi, and the other for the corresponding temperatures, Ti, all of dimension n ω g 1 ω1 ω ω 2 g2 ω = . ωg = . T = . . ω gn ω n
Non-linearity of the scale factor
The basic idea for measuring scale factor errors is to rotate the gyro at a known input rate ω, and compare this value with the measurement output of the gyro, ωg. The difference between both values is the error introduced by the non-linearity in the scale factor, ε. (1) ε =ωg −ω .
T1 T 2 . . Tn
(4)
where n is the number of rates of rotation multiplied by the number of temperature variations. The difference between the output of the gyro, ωgi, and the rate of rotation of the rotary table, ωi, represents the error of the gyro ε i at temperature Ti, (see Figure 3a). The next step is to find a mathematical model of the error ε as a function of the known variables: the output of
The same procedure has to be repeated at different speeds trying to cover the whole range of operation of the gyro(see Figure 1). The resulting error can be approximated by a thirdorder polynomial 1.5
2.5
1 1.5
90deg/sec
0.5
ε (deg/sec)
ε (deg/sec)
75deg/sec 0
-0.5
0.5
60deg/sec
-0.5 -60deg/sec
-1 -75deg/sec -1.5 -1.5
-2 -100
-90deg/sec
-50
0
ωg
50
-2.5 32.5
100
Figure 1: The error due to the non-linearity in the scale factor. The range of input rates in this and all subsequent experiments was –100°/s to +100°/s.
35
37.5
40
42.5
T ( °C )
(deg/sec)
Figure 2: Gyro measurement errors due to changes in temperature at different rates of rotation.
2
45
the gyro, ωg, and the temperature, T ε = f (T , ω g ) .
(5)
Figure 3 (a) shows experimental errors of the gyro at different rates of rotation and temperatures without any compensation. Finding an error model from the measurements, ωg and T, is a two-input single-output system identification problem. In order to use a general least-square algorithm, we adopted a Vandermonde matrix [9] that reduces the problem to a single-input single-output system. Figure 3b shows the errors of the gyro after correcting its output with the compensation function (see [3] for more details). a
3.
IMPLEMENTATION OF THE DEAD -
RECKONING NAVIGATION SYSTEM
To be useful, a dead-reckoning navigation system has to minimize its unbounded growth in position and orientation errors. This can be accomplished by meticulously modeling sensor errors and by the efficient design of a filter. In this paper we implement an indirect Kalman filter that estimates error states and overcomes some disadvantages of a conventional Kalman filter. The motivation for using this structure with differential drive mobile robots can be found in [10].
3.1
The error model for odometry b
The mobile robot position and heading angle are calculated from the output of incremental encoders by
Figure 3: Experimental errors of the gyro at different speeds and temperatures. (a)Without the compensation function (b) After applying the compensation function.
X ( k + 1) = X ( k ) + sin ψ (k ) × U ( k ) Y ( k + 1) = Y (k ) + cos ψ (k ) × U (k ) U (k ) - U L (k ) ψ(k+1 ) = ψ(k) + R D U R ( k ) + U L (k ) U (k ) = 2
(6)
where X and Y represent the mobile robot position in the navigation frame, and ψ is the heading of the mobile robot. U R and U L are the right and left wheel incremental distances respectively, and D is the distance between the left and right wheels (the so-called wheel separation distance). It is well-known that odometry is subject to systematic errors caused by factors such as unequal wheel-diameters, imprecisely measured wheel diameters (i.e., the so-called scale-factor error), or an imprecisely measured wheel separation distance (see [11] for a detailed discussion). Subject to these errors the robot’s position and its heading angle are computed by
U + S R (k)U R + U L + S L(k)U L Xˆ ( k + 1) = Xˆ ( k ) + sin ψˆ (k ) R 2 + + + U S (k)U U S (k)U R R R L L L Yˆ ( k + 1) = Yˆ ( k ) + cos ψˆ ( k ) (7) 2 (U R + S R (k)U R ) − (U L + S L (k)U L ) ψˆ ( k + 1) = ψˆ (k ) + D + δD ( k ) Xˆ ( k ) = X ( k ) + δ X ( k ) Yˆ ( k ) = Y ( k ) + δ Y ( k ) ψˆ ( k ) = ψ (k ) + δψ ( k )
where Xˆ ( k ), Yˆ ( k ) , and ψˆ (k ) are the calculated position and heading angle. X (k ), Y (k ) ,and ψ (k ) are the true position and heading angle. δX (k ), δY (k ) and δψ (k ) are the position and heading angle error. δD (k ) is the wheel separation distance error. SR (k) and SL (k ) are the scale factor errors of the right and left encoders, respectively. Subtraction of the true position values from the calculated position values yields the error propagation equations [6].
3
UR U U S R (k ) + sin ψ ( k ) L S L ( k )+ cos ψ ( k ) R 2 2 U U U δY ( k + 1) = δY ( k ) + cos ψ ( k ) R S R ( k ) + cos ψ ( k ) L S L ( k )− sin ψ ( k ) R 2 2 U U U -U δψ ( k + 1) = δψ ( k ) + R S R ( k ) − L S L (k ) + L 2 R δD (k ) D D D δ X ( k + 1) = δX ( k ) + sin ψ ( k )
where we assumed that δψ (k ) is small, i.e., cos δψ (k) ≅ 1 , sin δψ (k) ≅ δψ (k ) , δψ (k )SR (k ) ≅ 0 , δψ ( k )SL (k ) ≅ 0 , and D(k) >> δD(k) . It is also assumed that there is no error in wheel alignment. Left and right encoder scale factor errors and wheel separation distance errors are regarded as random constants due to their slow time-varying characteristics. Thus, these errors can be simply expressed by
(8)
φ$(k + 1) = φ$(k) + B$ s (k)Ω + B$ rb (k ) B$ (k ) = B + δB (k) s
s
s
B$ rb (k ) = Bb (k ) + δBrb (k) φ$(k ) = φ(k ) + δφ(k )
(10)
where φˆ( k ) is the calculated heading angle using the gyroscope. φ(k) is the true heading angle. Ω is the angular rate [volt]. B$ s ( k ) is the scale factor [°/sec/volt]. Bs is the
S R ( k + 1) = S R ( k ) S L ( k + 1) = S L ( k )
(9)
true scale factor. B$ rb ( k ) is the gyroscope bias. Bb (k ) is the deterministic bias. δφ (k ) is the gyroscope heading angle
δD( k + 1) = δD(k ).
For outdoor environments, zero-mean white Gaussian noise is added to (9) to represent ground irregularities, bumps, cracks, or slippage. These always add a positive error to the encoder reading, that is, the encoder will always add pulses but never miss pulses when encountering any such problem on the ground. In the error models, the random constants represent the average values of the irregular errors as well as the scale factor errors and wheel separation distance error, while the white Gaussian noise represent small deviation of the average values of the irregular errors.
3.2
+UL δψ ( k ) 2 +UL δψ ( k ) 2
The error model for the gyroscope
The heading angle from a gyroscope with bias drift and scale factor error is represented by (10)
error. δB s (k ) is the gyroscope scale factor error. δB rb (k ) is the gyroscope random bias error. A heading angle error equation for the gyroscope is obtained by subtracting φˆ(k + 1) from φ ( k + 1) , as shown in (11). Bias and scale factor error can be modeled as random constants.
δφ (k + 1) = δφ ( k ) + ΩδBs (k ) + δBrb (k ) δBs (k + 1) = δB s ( k ) δBrb (k + 1) = δBrb ( k ).
(11)
3.3 Implementation of the indirect Kalman filter Using the linear error models, the resulting state equations of the indirect feedback Kalman filter, x& = Ax + w , are as follows:
1 δX (k + 1) δY (k + 1) 0 δψ ( k + 1) 0 S k ( 1 ) + R S L ( k + 1) = 0 δD( k +1) 0 δφ ( k +1) 0 δBs ( k +1) 0 δB ( k + 1) 0 rb 0
UR +UL 2 U R +U L 1 − sin ψ (k ) 2 0
1
0
0
UR 2 UR cosψ ( k ) 2 UR D 1
0 0
0 0
0 0
1 0
0 1
0 0
0 0
0 0
0 0
0 0
0
0
0
0
0
0
cosψ (k )
where ω is the process noise.
sin ψ ( k )
UL 2 UL cosψ (k ) 2 UL − D 0
UL −UR D2 0
sin ψ (k )
0 0
0 0 0 δX ( k ) 0 0 0 δY ( k ) δψ (k ) 0 0 0 S ( k ) R 0 0 0 S L (k ) + w 0 0 0 δD(k ) 0 0 0 δφ (k ) 1 Ω 1 δBs (k ) 0 1 0 δBrb (k ) 0 0 1
( 12 )
Taking the heading angle difference between the incremental encoders and the gyroscope, the measurement equation Z = Hx + v is given by Z = ψˆ ( k ) − φˆ ( k ) + v = δψ ( k ) − δφ ( k ) + v =
[0
0
1
0
0
0
−1
0
δX (k ) δY (k ) δψ ( k ) ( 13 ) S R (k ) 0 ] S L ( k ) + v δD(k ) δφ ( k ) δB s (k ) δ B ( k ) rb
where ψˆ (k ) is a calculated encoder heading angle, φˆ( k ) is a calculated heading angle using a gyroscope, and v is measurement noise. It is assumed that w and v are white noise.
4.
EXPERIMENTAL RESULTS
In this section, experimental results performed on a paved parking lot are presented. The platform used in our experiments is the Pioneer AT four-wheel drive/skid steer mobile robot shown as the inset in Figure 4. The robot traveled along a pre-programmed, approximately square-shaped path, as also shown in Figure 4. The eight “via” points that are marked by a ‘+’ in Figure 4 were specified prior to the runs to define the path. The path planner then computed the angular and linear velocities to navigate the robot along the path. The
50
Preprogrammed via points
45 40
Y [m]
35 30
Robot path
25 20 15 10
Start position
5 0 -30
-20
Kalman_Gyro_results.ppt
-10
0
10
20
30
X [m]
Figure 4: Actual robot trajectory on the asphalted parking lot. In the experiment described here the robot performed five runs in clockwise (cw) and five runs in counter-clockwise (ccw) direction.
robot traveled at a maximum speed of 1.5 m/s, but it reduced its speed to 0.8 m/s near the via-points in order to minimize deviations from the path. Figure 4 shows the trajectories of the robot. We set two via-points at each corner to assure smooth turning. The robot traversed the 210-meters path in about three minutes and returned to the starting position. Upon returning to the starting position, the difference, D, between the robot’s actual position and the start position was measured with a measuring tape. The static bias drift error was determined before each run and subtracted from all gyro readings throughout the runs, for both the calibrated and uncalibrated runs. In the experiments without the calibration, the Kalman filter did not use the calibration information of the gyroscope either but estimated the scale factor error. In the experiments with the calibration, the Kalman filter did not estimate the scale factor error but used the outputs of the gyroscope with calibration. In the experiments without Kalman filter, the orientation of the robot was only determined from the output of the gyro, and not from odometry. We performed 10 individual, consecutive runs, five in clockwise (cw) and five in counter-clockwise (ccw) direction. The quantitative results of the experiments are summarized in Table II. The return position errors of Table II are the measured distance errors from the starting position (0, 0). As seen in Table II, when the measurements of the uncalibrated gyroscope were used, the performance of the positioning system is improved by using the Kalman filter since the Kalman filter compensates for the stochastic errors including the scale factor error of the gyroscope. However, when the measurements of the calibrated gyroscope were used, the improvement provided by the Kalman filter is relatively small compared to the improvement provided by our gyro calibration method. This is because our positioning system using only the gyro calibration is very accurate by itself and the stochastic errors in the sensors are very small. Moreover, in the dead-reckoning system, the distances are determined only by encoder data and the heading angle mostly depends on the gyroscope output. Our experiments demonstrate the relative effectiveness of the two accuracy-enhancing measures, gyro calibration and Kalman filter, discussed in this paper. Specifically, the results show these accuracy improvements: 2-fold with Kalman filtering alone; 7-fold with gyro calibration alone; 9-fold with gyro calibration and Kalman filtering.
4.
CONCLUSION
This paper presents two methods for improving the dead-reckoning accuracy of a mobile robot based on odometry and a fiber optic gyroscope. The first method is a meticulous calibration procedure for gyros. Our method measures errors for
Table II: Return Position Errors after completing five runs in both cw and ccw direction along the square path of Figure 4. Without Calibration
AIAA-98-4401, Boston, MA, Aug. 10-12, pp. 13151321. [2] KVH, 8412 W. 185th St., Tinley Park, IL 60477, USA, http://KVH.com. [3] Ojeda, L. Chung, H., and Borenstein, J., 2000, “Precision-calibration of Fiber-optics Gyroscopes for Mobile Robot Navigation.” Proc. of the 2000 IEEE Int. Conf. on Robotics and Automation, San Francisco, CA, April 24-28, 2000, pp. 2064-2069. [4] Chung, H., Ojeda, L. and Borenstein, J., 2000, “Accurate Mobile Robot Dead-reckoning with Precision-calibrated Fiber-optics Gyroscope.” Submitted for publication in IEEE Transaction on Robotics and Automation, February 2000. [5] Barshan, B., and Durrant-Whyte, H. F., 1995, “Inertial Navigation Systems for Mobile Robots.” IEEE Transaction on Robotics and Automation, June, pp. 328-342. [6] Komoriya, K., and Oyama, E., 1994, “Position Estimation of a Mobile Robot Using Optical Fiber Gyroscope(OFG).” 1994 Int. Conf. on Intelligent Robots and Systems(IROS ’94). Munich, Germany, Sept. 12-16, pp. 143-149. [7] Vaganay, J., Aldon, M. J., and Fourinier, A., 1993, “Mobile Robot Attitude Estimation by Fusion of Inertial Data,” Proc. of IEEE Int. Conf. on Robotics and Automation, Atlanta, GA, May 2-7, pp. 277282. [8] Park, K., Chung, H., and Lee, J., 1998, “Dead Reckoning Navigation for Autonomous Mobile Robots.” Proc .of Intelligent Autonomous Vehicle, Madrid, Spain, March 25-28, pp. 775-781. [9] Press, W. H., Flannery, B. P., Teukolsky, S. A., and Vetterling, W. T. 1982, “Numerical Recipes in FORTRAN: The Art of Scientific Computing.” 2nd ed. Cambridge, England: Cambridge Univ. Press, pp. 82-89. [10] Maybeck, P., 1979, Stochastic models, estimation, and control Volume 1, Academic Press, New York. [11] Borenstein, J. and Feng. L., 1996, “Measurement and Correction of Systematic Odometry Errors in Mobile Robots.” IEEE Transactions on Robotics and Automation, Vol. 12, No. 6, December, pp. 869880.
With Calibration
Without
With
Without
With
Kalman
Kalman
Kalman
Kalman
a. Clockwise [cm] Run 1
94.4
27.3
8.9
5.3
Run 2
80.2
23.6
9.8
8.7
Run 3
75.8
17.7
15.8
10.9
Run 4
92.5
19.4
9.4
4.7
Run 5
72.3
19.3
25.9
17.8
Average
83.0
21.5
14.0
9.5
b. Counter-clockwise [cm] Run 6
124.4
56.2
24.9
19.3
Run 7
146.8
81.4
21.8
18.2
Run 8
133.8
70.8
20.1
14.8
Run 9
111.4
35.4
10.8
9.9
Run 10
145.4
80.3
15
12.7
Average
132.4
64.8
18.5
15
static bias drift, non-linearity in the scale factor, and changes in temperature. A temperature-varying thirdorder polynomial function is defined based on the experimental data. The second method is the use of an indirect feedback Kalman filter. We developed a linear error state system model for implementing the indirect Kalman filter, in which the scale factor errors of the encoders, the distance error between the right and left wheels, and random bias and scale factor errors of the gyro were carefully modeled. The experimental results from multiple 210-meter runs around a paved parking lot show that the gyro calibration is most efficient, resulting in a 7-fold accuracy improvement. The Kalman filter alone provides only a 2-fold accuracy improvement. However, when both methods are used concurrently, they provide the largest (9-fold) accuracy improvement. We conclude that a calibrated fiber optic gyroscope is the single, most effect sensor modality for mobile robot dead-reckoning systems.
Acknowledgements This work was funded by DARPA under Award No DAAE07-98-C-L029 and by DOE under Award No. DEFG04-86NE37969. The authors wish to thank Dr. KyuCheol Park for his work on the Kalman filter.
REFERENCES [1] Bennett, S. M., Dyott, R., Allen, D., Brunner, J., Kidwell, R. and Miller, R., 1998, “Fiber Optic Rate Gyros as Replacements for Mechanical Gyros.” American Inst. of Aeronautics & Astronautics , 6
