2011 IEEE/RSJ International Conference on Intelligent Robots and Systems September 25-30, 2011. San Francisco, CA, USA
A Sensor Fusion Approach to Angle and Angular Rate Estimation Daniel Kubus and Friedrich M. Wahl
Abstract— Rotary encoders are the most common sensors to measure angles in mechatronic and robotic components, e.g., servo motors or robot joints. Especially, if encoders are not mounted on the motor shaft but on the output side of geared motors, high encoder resolutions are required. Resolution requirements may increase further if velocities are to be derived from the encoder signals, e.g., for motion control purposes. To avoid noise amplification problems when estimating angular rates from encoder signals, angular velocity sensors may be employed instead. However, a significant drawback of angular rate sensors – particularly of cheap MEMS gyroscopes – is their drift. Both the requirements on encoder resolution and the drift of the angular rate signal can be reduced significantly by the sensor fusion approach presented in this paper. The approach utilizes the encoder to eliminate the drift of the angular rate signal and integrates the resulting signal to obtain estimates of the angle. Apart from a detailed description of the approach and a theoretical analysis of its disturbance characteristics, various simulation and experimental results are presented demonstrating its effectiveness.
I. INTRODUCTION In robotics and mechatronics in general, encoders are widely used to determine angles in systems involving rotatory motion. Often, encoder signals are also employed to derive angular velocities and angular accelerations [1] as well. Regarding industrial manipulators, expensive highresolution encoders mounted on the motor shafts are commonly used to determine the joint angles. Although these encoders provide angle measurements with very low errors, the repetition errors of industrial manipulators are not negligible since gear effects, e.g., backlash, deteriorate accuracy. Therefore, state-of-the-art industrial manipulators also employ encoders on the output side of the gears to eliminate these errors. If the encoder measures the joint angle on the output side, very high encoder resolutions and hence expensive encoders are required – especially for highly dynamic motion control. Regarding static joint angle errors, however, a recent shift of paradigms in robot programming and manufacturing has relaxed resolution requirements as task tolerances are handled by sensors. System cost may hence be reduced considerably if the requirements on encoder resolution could also be relaxed for highly-dynamic motions. Sophisticated control approaches for industrial manipulators require the knowledge of the state of motion or at least Daniel Kubus and Friedrich M. Wahl are with the Institut f¨ur Robotik und Prozessinformatik, Technische Universit¨at Braunschweig, Germany,
[email protected],
[email protected] 978-1-61284-456-5/11/$26.00 ©2011 IEEE
the angle and the angular velocity. If encoders are used to provide angular velocity and angular acceleration signals, filters or estimators are generally required to reduce the noise in the velocity and acceleration signals. Regarding signals with low velocities, no encoder counts may occur during several sampling intervals. In this case, velocity estimation becomes especially delicate and sophisticated methods are required to provide meaningful estimates. Needless to say that increasing the encoder resolution is a viable solution in most cases but it comes at a non negligible cost. Therefore, direct angular rate measurement has become an attractive alternative with the advent of MEMS-based angular rate sensors. In contrast to expensive fiber optic gyroscopes [2], these sensors are affordable for most robotics applications. However, direct angular rate sensing with MEMS gyroscopes has a significant drawback: MEMS gyroscopes are not only affected by uncorrelated random noise but also by slowly time-varying disturbances denoted as drift. The characteristics of this random process have been analyzed in [3]. The combination of low-resolution encoders with inexpensive (MEMS) angular rate sensors to compensate the major disadvantage of either sensor may be a viable alternative to high-resolution encoders. The angular rate sensor can be utilized to virtually increase encoder resolution while the encoder can be employed to eliminate the drift of the angular rate sensor. This approach may be implemented using model-based or model-free approaches. Considering industrial applicability, model-free approaches that can be applied with minimum setup and tuning efforts are generally more desirable than model-based approaches that require indepth system knowledge. Therefore this paper introduces a model-free drift and offset compensator (DOC) employing FIR/IIR filtering techniques, which also lends itself to implementation in hardware (e.g. DSPs and FPGAs). Section II reviews related work. In Section III the concept of the DOC is presented and in Section IV its characteristics are analyzed. Section V shows the effectiveness of the approach in terms of encoder resolution enhancement and drift reduction with various simulations and experimental results. Section VI addresses further work and concludes the paper. II. RELATED WORK Estimating the state of motion of joints and other components involving linear translation or rotatory motion is generally a well-investigated problem. Commonly, linear/rotary encoders or resolvers are employed to obtain position signals. In [1] and [4] various approaches to real-time acceleration and velocity estimation based on encoder measurements are
2481
reviewed. Apart from approaches involving FIR/IIR filters with non-negligible delays, both distinguish between two major classes of methods: predictive postfiltering and linear state observers. While predictive postfiltering techniques (e.g. [5]) require explicit differentiation of the position signal, state observers (e.g. [6]), such as the linear Kalman filter [7], perform differentiation implicitly employing a system model. Other approaches utilize techniques involving numerical integration instead of differentiation to avoid the noise amplification problem connected to differentiation [8], [9]. Generally, estimation approaches may be based on either fixed-time or fixed-position evaluation of the encoder signal, i.e., the encoder counters are evaluated at specified instants of time or the time interval between the occurrences of a number of encoder counts (e.g. [4]) is measured. To combine the advantages of these approaches, smoothly transitioning between them depending on the operating conditions proves to be a viable solution, e.g. [10]. The aforementioned approaches rely on the spatial resolution of encoders to provide velocity estimates. Therefore, the estimation performance clearly deteriorates if the resolution of the encoder is reduced. Lee et al. [11] explicitly consider low-resolution encoders and low velocities resp. in velocity and position estimation. Apart from approaches aiming at the estimation of velocity and/or acceleration from encoder measurements, techniques that virtually increase the encoder resolution to improve the position accuracy exist. These techniques may be based on processing the standard square-wave quadrature output (e.g. polynomial fitting [12]) or involve calculations on the raw quadrature sinusoids of incremental encoders (e.g. phase detection [13]). Nevertheless, resolution remains a crucial performance factor if both position and velocity are to be estimated. To reduce encoder resolution requirements, direct angular rate sensing has become an affordable option. Compared to their expensive fiber optic counterparts, however, even stateof-the-art MEMS gyroscopes show drift rates that are orders of magnitude higher, cf. e.g. [14]. Therefore, sensor drift can significantly deteriorate system performance when angular rate signals are employed in control systems. Moreover, estimating angles based on angular rate measurements alone is hardly possible as the drift accumulates over time. This problem is usually tackled by combining gyroscopes with other sensors that are not affected by drift. As similar problems also arise in aircraft and spacecraft engineering, the respective literature, e.g. [15], provides valuable insights into this problem as well. Up to now, several approaches for linear and angular velocity estimation have been proposed that are based on fusing linear acceleration sensor and encoder signals. Zhu et al. have proposed different observer-based and frequencyweighting approaches to obtain velocity estimates from linear acceleration sensors and encoders [16]. In [17] encoder and acceleration sensor signals are fused by a Kalman filter. For angular velocity estimation, linear accelerometers only perform well with highly-dynamic motions or special setups. If the tangential acceleration is employed, high distances to
the center of rotation are required; if the centrifugal acceleration is exploited, high angular velocities are necessary for sufficient signal-to-noise ratios. Furthermore, commercially available linear acceleration sensors may also show considerable sensitivity to external disturbance and cross-coupling [18]. Therefore, this paper focuses on the combination of angular rate sensors and low-resolution encoders. III. THE DRIFT AND OFFSET COMPENSATOR A. Basic Concept The proposed approach exploits the remarkable accuracy with which angular differences can be determined by (inexpensive MEMS) gyroscopes within short time intervals to virtually increase encoder resolution. Conversely, it utilizes the high repeatability of encoders or rather the absence of accumulating errors in encoder signals to eliminate the drift of the angular rate signal. Fig. 1 illustrates the approach. Basically, the drift of the angular rate signal is estimated by analyzing the difference between the encoder output and a sliding integral of the angular rate signal. A low-order polynomial is fitted to this angle difference and its derivative is employed as a drift estimate. After eliminating the drift from the angular rate signal, it is integrated and fed to an offset compensator that eliminates remaining low-frequency deviations between the integrated angular rate signal and the encoder signal. gd
Gyroscope
g
gd
z-1 gdo
t Drift Compensator Rotary Encoder
Offset Compensator
e
Fig. 1. Simplified diagram of the DOC. First, the drift of the angular rate signal is estimated and eliminated by the drift compensator. Then, the resulting angular rate signal is integrated. In the final processing step, the offset compensator eliminates any remaining low-frequency disturbances in the integrated signal by an IIR filtering technique.
In robotic or mechatronic systems, the state of motion or – considering rotatory motion – at least the angle and the angular rate are required for various motion control approaches. As already mentioned above, angular rate estimation based on encoder signals becomes problematic if the encoder step size se is higher than the angle passed during the sampling interval ∆t, i.e., se > α˙ k ∆t (α˙k denotes the angular rate at time-step k) or in the same order of magnitude. Inexpensive angular rate sensors may help to overcome this problem but they show significant drift which prohibits integrating their output to estimate angles. Therefore, neither angular rate sensors nor low-resolution encoders alone can meet the requirements of demanding control systems. Even if both encoders and angular rate sensors are combined in one system, the previously mentioned problems remain to some
2482
extent. Precision control systems may pose high requirements on the encoder resolution or rather the quantization error, thus necessitating expensive (and often bulky) precision encoders. The drift of the angular velocity signal may lead to undesirable time-varying offsets in feedback loops thus deteriorating control performance. Regarding the signal characteristics of angular rate sensors based on MEMS, noise and drift can be separated well. While drift can be modeled as a highly-correlated and hence slowly time-varying random process, noise can be described as an uncorrelated random process with a rather continuous high-bandwidth power density spectrum. The proposed processing scheme does not aim at reducing the noise of angular rate sensors but at eliminating the drift. The drift estimator exploits the integral relationship between the angular rate and the angle measured by the encoder. More precisely, the angular rate signal is integrated within a sliding window and subtracted from the corresponding encoder signal. The resulting difference signal contains the information necessary to estimate and eliminate the sensor drift. If no sensor drift were present, the value of the difference signal would not change over time. Due to the drift, however, the difference signal varies over time. This variation can be approximated by a low-order polynomial and its first derivative can be exploited to eliminate the angular rate sensor drift. The corrected signal serves as both an estimate of the angular rate and the basis for the angle estimate. As the estimate of the drift is not error-free, the corrected angular rate signal is not completely free of drift. Hence, to obtain a valid angle estimate, the corrected angular rate signal cannot simply be integrated but an additional processing stage denoted as offset compensator must be employed to eliminate any remaining drift in the integrated signal. B. Drift Estimation and Elimination In this subsection, the details of the drift compensator are described. α ~ gk denotes the vector of angles obtained by backward integration of the gyroscope signal at time-step k within a window of N + 1 samples, see Eq. (1). The corresponding vector of encoder measurements α ~ ek is given by Eq. (2). α ~ gk
=
g g g T αk−N , αk−N +1 , . . . , αk
e e e T α ~ ek = αk−N , αk−N +1 , . . . , αk
(1) (2)
The elements of α ~ gk are given by Eq. (3). The integration procedure is performed backwards, i.e., it starts with the angular velocity α˙ g at time step k to simplify analysis and implementation. As only the derivative of αg contributes to the drift estimate, constants do not affect the result. Therefore, no integration constant is considered in Eq. (3). g αk−i = −∆t
k X
The drift estimation is based on the deviation ~ eα k between the encoder measurements and the integrated angular velocity signal. ~ gk (4) ~ ek − α ~ eα k =α eα eα k,i denotes element i (i = 0 . . . N ) of ~ k. To estimate the drift, a polynomial pqk (t, ~ ck ) is fitted to the deviation ~ eα where t denotes the time and ~ ck is the k vector of coefficients. pqk (t, ~ ck ) = ck,0 + ck,1 t + . . . + ck,q tq
Basically, any error measure can be employed to optimize and evaluate the quality of fit. Although not robust to outliers [19], the weighted least squares (WLS) estimate is employed. N X 2 (6) ~ ck = arg min βi pqk (i∆t, ~ ck ) − e α k,i ~ ck i=0 Due to the characteristics of the drift process [3], the application of higher order polynomials does not result in a significant improvement and hence a first-order polynomial is employed. The target function thus simplifies to
E=
N X
βi ck,1 ∆ti + ck,0 − eα k,i
i=0
2
(7)
A crucial advantage of the (W)LS estimate is that linear systems theory can be employed to analyze the system, cf. Section IV. Following [20], the best-line fit problem can be cast into matrix form. Since the sampling points are equidistant in time, the matrix Φ on the left does merely contain constants, i.e., τ , η, and γ. Merely the data matrix on the right requires the computation of the sums in each time-step. ! PN ∆t i=0 βi ieα ck,1 η τ k,i PN = (8) α ck,0 τ γ i=0 βi ek,i The weight-dependent constants in the matrix Φ are given by: γ=
N X
βi
τ = ∆t
N X
βi i
η = ∆t2
i=0
i=0
N X
β i i2
(9a-c)
i=0
As the variance of the elements of ~ eα k is not constant, i.e., the variance decreases with increasing i due to the backward integration of the noisy angular velocity α˙ g , the ordinary LS estimate (βi = 1) is not the best linear unbiased estimator but each residual has to be weighted with the reciprocal of the variance V ar(eα k,i ). No correlation between the random variables is assumed here for simplicity although the noise of the IMU is non-white and hence shows correlation. The variance V ar(eα k,i ) can be approximated by [21]: V ar(eα k,i )
α˙ jg
(5)
=
(3)
j=k−i
=
∆t denotes the sampling time.
V ar ∆t ∆t2
k X
j=k−N +i
j=k−N +i
2483
k X
α˙ jg
e + αk−N +i (10)
e V ar(α˙ jg ) + V ar(αk−N +i )
Assuming a constant variance of the individual measurements V ar(α˙ jg ) = (σ g )2 and V ar(αje ) = (σ e )2 the variance equation can be simplified to 2 g 2 e 2 V ar(eα k,i ) = ∆t (σ ) (N − i + 1) + (σ )
To determine the drift estimate, the time derivative of the polynomial pqk is employed. d c˜k = pqk (t, ~ ck ) (12) p˙ qk t, ~ dt ~ c˜k denotes the reduced coefficient vector after differentiation. Since this description focuses on first-order polynomials, the drift rate estimate is given by a constant, i.e., the coefficient ck,1 . The calculation of ck,0 is thus rendered unnecessary. d ck ) = ck,1 (13) c˜k = p1k (N ∆t, ~ p˙ qk N ∆t, ~ dt Solving Eq. (8) for ck,1 yields: PN PN α γ∆t i=0 βi ieα k,i − τ i=0 βi ek,i ck,1 = (14) γη − τ 2 Now, the angular velocity sensor output can be corrected by the estimated drift. =
α˙ kg
+
p˙ qk
(N ∆t, c˜k ) =
α˙ kg
+ ck,1
(15)
The resulting angular rate α˙ kgd , k ≥ 0 can be used for control purposes but also serves as the basis of the angle estimate. gdo αkgd = α˙ kgd ∆t + αk−1 gdo αk−1
is the output of the previous time step and
(16) gdo α−1
= α0e .
C. Offset Compensation Since errors in the drift estimate may accumulate, consistency between αgd and αe has to be ensured. For this purpose, a primitive frequency-weighting filter denoted as offset compensator is employed. This IIR filter computes a gdo difference vector d~α and the k between the DOC outputs α e encoder measurements α in a sliding window and calculates a correction signal based on the sum of differences. It works as a high-pass filter for αgd and as a low-pass filter for αe . Further details are presented in the following section. M is the filter order. The difference vector d~α k is given by Eq. (19). h iT gdo gdo gdo α ~ gdo = αk−M , αk−M (17) k +1 , . . . , αk
gd
gd
ad
gdo
ado
Gyroscope
S1 t e
Rotary Encoder
(11)
The weights βi contained in the data vector and the −1 constants of the matrix Φ are given by βi = V ar(eα . k,i ) 2 g 2 e 2 As (N + 1) ∆t (σ )
