comparative study on state estimation in elastic...

12
COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi Tomizuka ABSTRACT In robots with elastic joints, discrepancies exist between the motor side and the load side (i.e., the link of the robotic joint) due to joint elasticity.Thus, sensing only at the motor side does not precisely provide the information of interest at the load side. In view that the load side states determine the position and orientation of the end-effector, it is critical to have the load side states sensed and/or estimated. In this paper, we present a comprehensive comparative study on load side state estimation utilizing two different models of robotic elastic joints. Low-cost MEMS inertia sensors, such as gyroscopes and accelerometers, are adopted on the load side to supplement the motor side sensing. We also incorporate sensor noises and biases in the measurement dynamics. Kalman filtering methods are designed based on the extended dynamic/kinematic models for the fusion of multiple sensor signals. We also study the specific issue related to the noise covariance adaptation. The effectiveness of the proposed schemes is experimentally demonstrated and compared on a robotic elastic joint testbed. Key Words: Elastic joint, Kalman filtering, covariance adaptation. I. INTRODUCTION In control applications, discrepancies between the avail- able measurements and the required information set difficul- ties in achieving good control performance. Such phenomena result from sensor dynamics as well as plant dynamics. Par- ticularly, in robots with joint elasticity, due to gear compli- ances and nonlinearities, the ideal kinematic relationship (e.g., the speed reduction ratio) between the load side (i.e., the link of the robotic joint) and the motor side does not hold. This results in discrepancies between the actual physical states of the load side and the measurements at the motor side. Therefore, good motor side performance based on motor side measurements alone does not guarantee the desired good load side performance [1], which is a critical issue in practical applications. The early work on the control of elastic joint robots dates back to the late 1980s. In [2], the dynamic model of elastic joint robots was formally introduced and the feedback linearization approach was proposed to control such robots with full state feedback including the load side states. Later it was claimed that a simple PD controller [3] or a set-point regulation controller [4], using motor side state feedback only, can globally stabilize robots with elastic joints. However, mismatches between the kinematics and dynamics of the actual robot and those of a model simplification deteriorate the performance of the motor side state feedback control due to the dependance on the dynamic model (e.g., gravity torques and joint stiffness). Therefore, the load side state feedback is essential for good control performance, especially for accurate positioning of the end-effector and suppressing the vibration/resonance induced by joint compli- ances. Recently, several control approaches such as active disturbance rejection [5], multi-variable control loops [6], and linear pole placement [7], were proposed for vibration sup- pression, all of which confirmed the necessity of load side information feedback. Precise load side position measurements (e.g., load side encoder), however, are normally not available in industrial robots due to cost and assembly issues. Note that the load side resolution of the motor side encoder is improved by a factor of gear ratio. Thus, the load side encoder with the same load side resolution is normally much larger and much more expensive than the motor side encoder. To overcome this problem, inexpensive MEMS inertia sensors that are easy to mount may be considered on the load side instead of large and expensive load side encoders. A representative cost compari- son between the encoders and the MEMS inertia sensors can be found in [8]. Consideration should be given, however, to problems such as non-negligible biases, limited bandwidth, and noises from low-cost sensors, which set restrictions on the direct utilization of sensor signals. These problems may be circumvented by the proper fusion of multiple sensor signals, to achieve clean and precise load side position esti- mation with substantial improvement over the motor side encoder measurements. Furthermore, the utilization of inertia sensors such as accelerometers and gyroscopes enables the Manuscript received September 29, 2012; revised January 28, 2013; accepted March 31, 2013. Wenjie Chen (corresponding author, e-mail: [email protected]) and Masayoshi Tomizuka (e-mail: [email protected]) are with Department of Mechanical Engineering, University of California, Berkeley, CA 94720, USA. This work was supported by FANUC Corporation, Japan. Real-time control hard- ware and software were provided by National Instruments, Inc. Asian Journal of Control, Vol. 16, No. 3, pp. 818–829, May 2014 Published online 26 June 2013 in Wiley Online Library (wileyonlinelibrary.com) DOI: 10.1002/asjc.755 © 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Upload: others

Post on 21-May-2020

13 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS

Wenjie Chen and Masayoshi Tomizuka

ABSTRACT

In robots with elastic joints, discrepancies exist between the motor side and the load side (i.e., the link of the robotic joint)due to joint elasticity. Thus, sensing only at the motor side does not precisely provide the information of interest at the load side.In view that the load side states determine the position and orientation of the end-effector, it is critical to have the load side statessensed and/or estimated. In this paper, we present a comprehensive comparative study on load side state estimation utilizing twodifferent models of robotic elastic joints. Low-cost MEMS inertia sensors, such as gyroscopes and accelerometers, are adoptedon the load side to supplement the motor side sensing. We also incorporate sensor noises and biases in the measurement dynamics.Kalman filtering methods are designed based on the extended dynamic/kinematic models for the fusion of multiple sensor signals.We also study the specific issue related to the noise covariance adaptation. The effectiveness of the proposed schemes isexperimentally demonstrated and compared on a robotic elastic joint testbed.

Key Words: Elastic joint, Kalman filtering, covariance adaptation.

I. INTRODUCTION

In control applications, discrepancies between the avail-able measurements and the required information set difficul-ties in achieving good control performance. Such phenomenaresult from sensor dynamics as well as plant dynamics. Par-ticularly, in robots with joint elasticity, due to gear compli-ances and nonlinearities, the ideal kinematic relationship(e.g., the speed reduction ratio) between the load side (i.e., thelink of the robotic joint) and the motor side does not hold.This results in discrepancies between the actual physicalstates of the load side and the measurements at the motor side.Therefore, good motor side performance based on motor sidemeasurements alone does not guarantee the desired good loadside performance [1], which is a critical issue in practicalapplications.

The early work on the control of elastic joint robotsdates back to the late 1980s. In [2], the dynamic model ofelastic joint robots was formally introduced and the feedbacklinearization approach was proposed to control such robotswith full state feedback including the load side states. Later itwas claimed that a simple PD controller [3] or a set-pointregulation controller [4], using motor side state feedbackonly, can globally stabilize robots with elastic joints.However, mismatches between the kinematics and dynamics

of the actual robot and those of a model simplificationdeteriorate the performance of the motor side state feedbackcontrol due to the dependance on the dynamic model (e.g.,gravity torques and joint stiffness). Therefore, the load sidestate feedback is essential for good control performance,especially for accurate positioning of the end-effector andsuppressing the vibration/resonance induced by joint compli-ances. Recently, several control approaches such as activedisturbance rejection [5], multi-variable control loops [6], andlinear pole placement [7], were proposed for vibration sup-pression, all of which confirmed the necessity of load sideinformation feedback.

Precise load side position measurements (e.g., load sideencoder), however, are normally not available in industrialrobots due to cost and assembly issues. Note that the load sideresolution of the motor side encoder is improved by a factorof gear ratio. Thus, the load side encoder with the same loadside resolution is normally much larger and much moreexpensive than the motor side encoder. To overcome thisproblem, inexpensive MEMS inertia sensors that are easy tomount may be considered on the load side instead of large andexpensive load side encoders. A representative cost compari-son between the encoders and the MEMS inertia sensors canbe found in [8]. Consideration should be given, however, toproblems such as non-negligible biases, limited bandwidth,and noises from low-cost sensors, which set restrictions onthe direct utilization of sensor signals. These problems maybe circumvented by the proper fusion of multiple sensorsignals, to achieve clean and precise load side position esti-mation with substantial improvement over the motor sideencoder measurements. Furthermore, the utilization of inertiasensors such as accelerometers and gyroscopes enables the

Manuscript received September 29, 2012; revised January 28, 2013; acceptedMarch 31, 2013.

Wenjie Chen (corresponding author, e-mail: [email protected]) andMasayoshi Tomizuka (e-mail: [email protected]) are with Department ofMechanical Engineering, University of California, Berkeley, CA 94720, USA.

This work was supported by FANUC Corporation, Japan. Real-time control hard-ware and software were provided by National Instruments, Inc.

Asian Journal of Control, Vol. 16, No. 3, pp. 818–829, May 2014Published online 26 June 2013 in Wiley Online Library (wileyonlinelibrary.com) DOI: 10.1002/asjc.755

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 2: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

direct measurement and/or estimation of the load side accel-eration and velocity, which cannot be properly captured by aposition encoder only.

The algorithms for load side state estimation have beenextensively studied for robots with joint elasticity. In [9], areduced order robust high gain observer was designed basedon the robot dynamic model to estimate the load side positionand velocity using motor side measurements only. In [10], a“semi-dynamic” model based observer was designed usingmotor encoder and load side accelerometer measurements.With the load side accelerometer measurement, the observeris decoupled into each joint and relies on only the motor andreducer related dynamic parameters. Recently, an extended-state observer was introduced in [11] to estimate thetransformed coordinates (i.e., load side position, velocity,acceleration, and jerk), in the presence of model uncertaintyand external disturbance. However, the performance and sta-bility of this observer may be poor when uncertainty/disturbance is fast varying. In [12], an extended Kalman filter(EKF) based on the dynamic model was proposed to estimatethe load side states using motor side measurements. Thecomplex robot dynamics and model uncertainty, however,greatly hinder the application of such EKF schemes to high-DOF (degree of freedom) robots.

The aforementioned methods are all based on the robotdynamic model requiring accurate system parameters, andthus are not reliable when subject to model uncertainties. Toreduce the dependance on the dynamic model, some haveapproached a simpler and more accurate model representa-tion, i.e., the kinematic model. In [13,14], it was suggestedthat a Kalman filtering method based on the kinematic model(KKF) can be formulated by making use of accelerometersand position encoders. The model uncertainties are avoidedby using the kinematic relation between the position and theacceleration. This method, however, was developed mainlyfor one-mass systems or direct drives (i.e., without thejoint compliance due to gear transmissions). Also, the mainpurpose was velocity estimation while assuming the positionmeasurement is available.

Moreover, in most of the aforementioned methods,noise covariances were used as design parameters, which arenot easy to select intuitively when multiple measurements areutilized. Non-negligible bias effect and limited bandwidthwere also often ignored. Thus, it is desired to derive theestimation algorithms for robots with joint elasticity withfurther consideration of the physical sensor limitations.

In our previous work [15], we studied such estimationalgorithms in two categories: dynamic model based and kin-ematic model based. Measurement dynamics were consideredinto the dynamic and kinematic models to deal with sensornoises and biases. The estimation algorithms were formulatedbased on these extended models to fuse the measurementsfrom both the motor side and the load side.Adaptation of noise

covariances was also addressed. In this paper, a more compre-hensive comparative study for these estimation algorithms willbe presented for elastic joints in the absence of precise loadside position sensing. Complete formulation of the estimationschemes utilizing two different models (i.e., dynamic andkinematic) will be given. A comparative experimental study ispresented to validate the proposed schemes. The derivation ofthe covariance adaptation algorithm will be detailed. Theschemes will also be discussed with potential extensions toaccommodate non-negligible nonlinear dynamics.

Nomenclatureθℓ Load side positionθℓ,s Sensor measurement of load side position, θℓ��θ ,s Sensor measurement of load side velocity, �

��

�θ ,s Sensor measurement of load side acceleration, ���θ

θℓf Filtered load side positionθℓf,s Fictitious measurement of filtered load side position, θℓf

θm Motor side positionθm,s Sensor measurement of motor side position, θm

bv Bias of gyroscope measurement, ��θ ,s

ba Bias of accelerometer measurement, ���θ ,s

u Control input, i.e., motor torqueus Sensor measurement of motor torque, unu Measurement noise of motor torque, unsm Measurement noise of motor side position, θm

nvℓ Measurement noise of load side velocity, ��θ

naℓ Measurement noise of load side acceleration, ���θ

nba Fictitious noise of accelerometer bias, ba

nbv Fictitious noise of gyroscope bias, bv

nℓf Fictitious noise of filtered load side position(measurement), θℓf,s

dj Joint damping of the elastic jointdm Motor side viscous damping coefficientdℓ Load side viscous damping coefficientJm Motor side inertia of the elastic jointJℓ Load side inertia of the elastic jointkj Joint stiffness of the elastic jointN Gear reduction ratio of the elastic joint

II. ESTIMATION ALGORITHMS

2.1 Model for measurement dynamics

The bias and the noise in the sensor output can bedescribed as

�b t n tb( ) = ( ) (1a)

y t y t b t n ts s( ) = ( ) + ( ) + ( ) (1b)

where b is the sensor bias varied by the fictitious noise, nb. yis the actual physical quantity to be measured, ys the sensoroutput, and ns the measurement noise.

819W. Chen and M. Tomizuka: State Estimation in Elastic Joints

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 3: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

2.2 Model for system dynamics

A typical robotic elastic joint system can be modeled asa two-mass system, as shown in Fig. 1. The subscripts m andℓ denote the motor side and the load side quantities, respec-tively. d represents the viscous damping coefficient, and J isthe moment of inertia. θ and u represent the angular positionand the motor torque, respectively, and kj and dj are the stiff-ness and the damping coefficients of the gear reducer. Thegear ratio of the reducer is denoted by N.

Equations of motion for the system in Fig. 1 are

J d uN

kN

dN

m m m m jm

jm�� �

��

� �θ θ θ θ θ θ+ = − ⋅ −⎛⎝⎜

⎞⎠⎟ + −⎛

⎝⎜⎞⎠⎟

⎡⎣⎢

⎤⎦⎥

1(2a)

J d kN

dN

jm

jm

� � � � � ��� �

��θ θ θ θ θ θ+ = −⎛

⎝⎜⎞⎠⎟ + −⎛

⎝⎜⎞⎠⎟

(2b)

The state-space model representation of this elasticjoint can be formulated as

�x x u= +A B . (3)

where

x m m= [ ]θ θ θ θ� �� �

T

A =

−−

+

−−

+

0 1 0 0

0 0 0 1

2

2k

N J

d d N

J

k

NJ

d

NJ

k

NJ

d

NJ

k

J

d d

j

m

m j

m

j

m

j

m

j j j j

� � �

JJ �

⎢⎢⎢⎢⎢⎢⎢

⎥⎥⎥⎥⎥⎥⎥

B = ⎡⎣⎢

⎤⎦⎥

01

0 0Jm

T

The experimental setup of a robotic elastic joint in theMechanical Systems Control Laboratory at the University ofCalifornia, Berkeley, is shown in Fig. 1. As shown in thefigure, the load side is equipped with both a gyroscope and aset of accelerometers. In this case, we introduce two statevariables, ba and bv representing the biases of accelerometerand gyroscope outputs with the fictitious noises as nba and nbv,respectively. Three measured signals are treated as the systemoutputs, i.e., the motor side position (θm,s), the load sidevelocity and acceleration ( �

�θ ,s and ���θ ,s), where •s denotes the

sensor measurement of the actual physical quantity •. u in (3)represents the actual motor torque, which may differ from thecommanded or measured torque us. It is assumed that u and us

are related by us = u + nu (or u = us − nu) where nu is a ficti-tious noise.

After these considerations, the augmented dynamicmodel for the system in Fig. 1 becomes

�x t x t u t w tm m m m u s m w m( ) ( ) ( ) ( ), ,= + +A B B (4a)

y t x t v tm m m m( ) ( ) ( )= +C (4b)

where

x x b bm a v= [ ] ∈ ×T T R6 1

ym m s s s= ⎡⎣ ⎤⎦ ∈ ×θ θ θ, , ,� ��� �

T R3 1

w n n nm u ba bv= −[ ] ∈ ×T R3 1

v n n nm sm v a= [ ] ∈ ×� �

T R3 1

A0

0 0m

A=

⎣⎢

⎦⎥ ∈ ×R6 6

B 0m u B, = [ ] ∈ ×T T R6 1

B0

0m w

B

I, =⎡

⎣⎢

⎦⎥ ∈ ×

2

6 3R

Cm

A

=⎡

⎢⎢⎢

⎥⎥⎥

∈ ×

1 0 0 0

0 0 0 1

0 0

0 1

1 04

3 6R .

Here, A4 is the fourth row of the matrix A, and In is ann × n identity matrix. nsm, nvℓ, and naℓ are the measurement

Fig. 1. A robotic elastic joint with gear transmission (theharmonic drive introduces a mismatch between thepositions of the motor and the payload). The constructionof this testbed will be detailed in Section 3.1.

820 Asian Journal of Control, Vol. 16, No. 3, pp. 818–829, May 2014

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 4: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

noises of motor side position, θm, load side velocity, ��θ , and

load side acceleration, ���θ , respectively.

The augmented model (4) naturally leads to a dynamicmodel-based Kalman filter with us as the input and ym as theoutput. Instead of using a dynamic model, another way toconstruct a Kalman filter is to use a kinematic model, asdetailed in the next section.

2.3 Model for system kinematics

The kinematic model from the acceleration to the posi-tion on the load side can be written as

d

dt

t

t

t

t

θθ

θθ

�� �

�( )

( )

( )

( )⎡⎣⎢

⎤⎦⎥

= ⎡⎣⎢

⎤⎦⎥

⎡⎣⎢

⎤⎦⎥

+ ⎡⎣⎢

⎤⎦⎥

0 1

0 0

0

1���θ ( )t (5)

The Kalman filter based on the kinematic model (5) iscalled the kinematic Kalman filter (KKF) [13]. In KKF, theacceleration measurement is used as an input to the filter, andthe position measurement is used to correct the estimationoutput. In the robotic elastic joint, however, precise load sideposition measurement (e.g., load side encoder) is normallynot available, which makes it difficult to directly utilize theKKF algorithm.

Here, an original but intuitive method is proposed toapproximate the load side position measurement. Let Gm2ℓ(s)be the transfer function from the motor side position θm to theload side position θℓ, i.e.

G ss

s

d s k

N J s d d s km

m

j j

j j2 2�

� �

�( ) ( )( )

=+

+ + +[ ]θθ ( )

by the inherent system dynamics (2). Thus, Gm2ℓ(s) isapproximately zero-phase static gain at low frequencies, sincethe dynamic chain from θm to θℓ is modeled by mass, gear,spring, and damper. This indicates that the low frequencycomponent of θℓ can be approximated by that of θm.

Pass θm and θℓ through a first order low pass filter

G ss

f ( ) =+α

α, where α = βfb, fb is the bandwidth of Gm2ℓ(s),

and β ∈ (0, ∞) is a design parameter for the low pass filter.Let the filter outputs be θmf and θℓf, respectively. It follows that

�θ αθ αθmf mf m= − + (6a)

�� � �θ αθ αθf f= − + (6b)

Therefore, if β is chosen properly, the filter outputs havethe following relation

θ λ θ�f mf≈ 0 (7)

where λ01=N

is the DC gain of Gm2ℓ(s). Generally, it is

desired that β ≤ 1 and thus α ≤ fb. If the low frequency

components of θm and θℓ are highly dominant, however, β canbe chosen to be larger than 1 to increase the filter convergencerate.

The filter dynamics (6) and the measurement dynamics(1) can be integrated into the system kinematic model (5),giving

� ���x t x t t w tk k k k u s k w k( ) ( ) ( ) ( ), , ,= + +A B Bθ (8a)

y t x t v tk k k k( ) ( ) ( )= +C (8b)

where

x b bk f a v= −⎡⎣ ⎤⎦ ∈ ×θ θ θ� � �� T R5 1

yk f s s= ⎡⎣ ⎤⎦ ∈ ×θ θ� ��

, ,T R2 1

w n n nk a ba bv= − −[ ] ∈ ×�

T R3 1

v n nk f v= [ ] ∈ ×� �

T R2 1

A0

0 0

k =

−⎡

⎢⎢⎢⎢

⎥⎥⎥⎥

∈ ×

α α 0 0

0 0 1 0

0 0 0 15 5R

Bk u, = [ ] ∈ ×0 0 1 0 0 5 1T R

B 0k w I, = [ ] ∈ ×3

5 3T R

Ck = ⎡⎣⎢

⎤⎦⎥

∈ ×1 0 0 0 0

0 0 1 0 12 5R

and nℓf is the fictitious noise of the filtered load side position(measurement), θlf,s.

By the approximation in (7), λ0θmf,s, whereθmf,s(s) = Gf(s)θm,s(s), can be used as the fictitious measure-ment for the model output θℓf,s. The noise covariance of θℓf,s

can be approximated by that of λ0θmf,s, or used as a designparameter for the following Kalman filter.

2.4 Kalman filtering

The discrete time form of the extended system model(4) or (8) can be formulated as

x k x k u k w kd d d d u d d w d( ) ( ) ( ) ( ), ,+ = + +1 A B B (9a)

y k x k v kd d d d( ) ( ) ( )= +C (9b)

where xd(k), yd(k), ud(k), wd(k), and vd(k) are the k-th time stepsampled values of xm(t), ym(t), us(t), wm(t), and vm(t) for the

821W. Chen and M. Tomizuka: State Estimation in Elastic Joints

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 5: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

system dynamic model (4), or xk(t), yk(k), ���θ , ( )s t , wk(t), and

vk(t) for the system kinematic model (8), respectively. Ad,Bd,u, Bd,w, and Cd are derived from (4) or (8) by thezero-order-hold (ZOH) method [16]. In practice, Bd,wwd couldbe generalized to include the disturbance and/or mismatchedmodel dynamics.

For the extended system model (9), a Kalman filter toestimate the system states is given by

ˆ ( ) ˆ ( ) ( ) ( )x k x k L k y kd do

d do= + � (10a)

ˆ ( ) ˆ ( ) ( ),x k x k u kdo

d d d u d+ = +1 A B (10b)

�y k y k x kdo

d d do( ) = ( ) − ( )C ˆ (10c)

where the Kalman filter gain Ld is calculated as

L k M k M k R kd d d d d d d( ) = ( ) ( ) + ( )⎡⎣ ⎤⎦−

C C CT T ˆ 1(11a)

M k Z k Q kd d d d d w d d w+( ) = ( ) + ( )1 A A B BT, ,

Tˆ (11b)

Z k M k L k M kd d d d d( ) ( ) ( ) ( )= − C (11c)

and ˆ ( )Q kd and ˆ ( )R kd are the covariance estimates of wd(k) andvd(k), respectively. Md(k) and Zd(k) are the one-step a priori anda posteriori covariances of the state estimation error, respec-tively. It can be checked that (Ad, Cd) is observable and (Ad,Bd,w) is controllable. The controllability by the disturbanceinput (e.g., wd) rather than the control input (e.g., ud) is impor-tant for the stationary property of Kalman filter. Note that (Am,Bm,w) and (Ak, Bk,w) are controllable, and their ZOH equivalent(Ad, Bd,w) is also controllable when the sampling frequency(i.e., 1 kHz) is specified.The observability of the discrete-timepair (Ad, Cd) can be checked similarly. Thus, if wd and vd arestationary zero-mean Gaussian white noises, Ld, Md, and Zd

will converge to stationary matrices. However, wd and vd areoften interpreted as fictitious noise terms and their covariancesare adjusted to assign desired closed loop eigenvalues to theestimator [14]. In practice, they are chosen such that theestimator dynamics are five to ten times faster than that ofthe controller or fast enough to suppress the effect of pertur-bations [14]. Another way to deal with these covariances is touse the adaptive scheme described in the next section.

Remark 1. If only the accelerometer measurement is avail-able on the load side for the estimation purpose, i.e., nogyroscope measurement in the system output (4) or (8), theproposed method can still be formulated in a similar way.However, it is better to include the gyroscope measurement,since θℓf,s uses a fictitious measurement λ0θmf,s to formulateKKF, and �

�θ ,s is the only load side real measurement in theKKF model output in (8). The benefits of this additionalgyroscope will be demonstrated experimentally in Section III.

Remark 2. The KKF has several advantages [14] comparedto the dynamic model based Kalman filter (DKF). First, thesystem representation using the kinematic model is simplerthan the one using the dynamic model. Secondly, the kinematicmodel is an exact representation of the system states. Itinvolves neither dynamic parameters nor external distur-bances. Thus no model uncertainties need to be considered inthe KKF, while the performance of the DKF method is subjectto model uncertainties and nonlinear dynamics (e.g., Coulombfriction and transmission error [17]) that are ignored whenutilizing the linear dynamic model.

2.5 Estimation of noise covariance

As an optimal stochastic estimator, the Kalman filterassumes linear model and Gaussian additive noises withknown covariances. In reality, however, it is often difficult toget the accurate covariances of the process or measurementnoises. Thus, particular interest has been focused on the esti-mation of the noise covariances, i.e., ˆ ( )Q kd and ˆ ( )R kd .

In the proposed Kalman filter, the bias noises (nba andnbv) and the output θℓf,s are fictitious and their covariancescannot be physically identified. Practically, if the measure-ment bias is time invariant or slowly varying, it is desired thatthe fictitious bias covariance should be large enough at thebeginning to steer the bias estimate quickly to its actual value.Then the fictitious noise covariance can be decreased toreduce the sensitivity of the bias estimation.

To handle uncertainties in these covariances, an adap-tive approach utilizing the residual information for covari-ance estimation is applied. The routines presented here aremodified from the algorithms summarized in [18,19]. Thealgorithm derivation using covariance matching technique isdetailed in Appendix A.

By comparing the a priori and the a posteriori stateestimates, the process noise covariance estimate ˆ ( )Q kd isupdated by

ˆ*( ) [ ( ) ( ) ( )

( ) ]( ),

,

Q k x k x k Z k

Z kd d w d d d

d d d d w

= +− −

B

A A B

1

11

Δ Δ T

T T(12a)

Δx k x k x kd d do( ) = ( ) − ( )ˆ ˆ (12b)

ˆ ( ) ˆ ( ) [ ˆ*( ) ˆ ( )]Q k Q k Q k Q k Nd d d d Q+ = + −1 / (12c)

where the adaptive estimate ˆ*( )Q kd is shown to be the residualpseudo-covariance plus the change between the a priori andthe a posteriori covariances for the current time step. Thenext step covariance estimate ˆ ( )Q kd +1 is then updated to thisestimate ˆ*( )Q kd

in an exponential moving average mannerwith NQ as the window size. Note that, if Bd,w is not invertible,Bd w,

−1 in (12a) can be replaced by the followingpseudo-inverse

822 Asian Journal of Control, Vol. 16, No. 3, pp. 818–829, May 2014

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 6: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

BB B B B

B B B Bd w

d w d w d w d w

d w d w d w d

,, , , ,

, , ,

, ;

,

† =( )

( )

T T

T T

is thin1

1,, .w is fat

⎧⎨⎪

⎩⎪(13)

Similarly, the measurement noise covariance estimate,ˆ ( )R kd , can be adaptively updated by

ˆ*( ) ( ) ( ) ( )R k y k y k Z kd d d d d d= +Δ Δ T TC C (14a)

Δy k y k x kd d d d( ) = ( ) − ( )C ˆ (14b)

ˆ ( ) ˆ ( ) [ ˆ*( ) ˆ ( )]R k R k R k R k Nd d d d R+ = + −1 / (14c)

where Δyd(k) is not the innovation but the a posterioriestimation error. The physical interpretation of this solution isthat the theoretical value of measurement noise covarianceshould match with the estimation residual covariance (seeAppendix A).

Remark 3. The above adaptation for the process (or meas-urement) noise covariance estimate, Q̂d (or R̂d), is the sub-optimal solution developed separately assuming that the actualcovariance Rd (or Qd) is known [19] (seeAppendixA). The twosolutions can be used, with caution, to estimate both covari-ances simultaneously, or can be implemented one after anotherin the serial way, or can be confined to adapt Q̂d (or R̂d) only.

Remark 4. The estimation performance is sensitive to thewindow sizes of the moving average filters. Thus, the windowsizes should be carefully selected for each application. Some-times it is preferred to use small window sizes at the beginningto speed up the covariance adaptation process. After thecovariances have converged, the window sizes can be enlargedto maintain smooth estimation performance.

III. EXPERIMENTAL STUDY

3.1 Experimental setup

The proposed methods are implemented on a single-DOF elastic joint testbed shown in Fig. 1. This experimentalsetup consists of: (i) a servo motor with a 20,000 counts/revolution encoder; (ii) a harmonic drive with a 80:1 gear ratio;(iii) a load-side 144,000 counts/revolution encoder; and (iv)and a payload. The anti-resonant and resonant frequencies ofthe setup are approximately 11 Hz and 19 Hz, respectively.

The load side encoder is used only for performanceevaluation. Besides the encoder, two other sensors are avail-able on the load side. A MEMS gyroscope (Analog,ADXRS150) is installed at one end of the payload. Twoaccelerometers (Kistler, 8330A3) are installed at the two endsof the payload symmetrically as shown in Fig. 1 to compen-sate for the gravity effects on the measurements. Finally, the

algorithms are implemented in a LabVIEW real-time targetinstalled with LabVIEW Real-Time and FPGA modules. Thesampling rate is selected as 1 kHz.

The noise of θm,s is bounded by the encoder resolutionδθm,s. This gives the approximate noise variancevar( ) ,nsm m s= δθ 2 12 [14]. The noise variances for ��

�θ ,s and ��θ ,s

can be obtained by zero-acceleration and zero-velocityexperiments. The fictitious noise variances of θℓf,s, us, ba, andbv, i.e., var(nℓf,s), var(nu), var(nba), and var(nbv), cannot bedetermined experimentally (no torque measurement is avail-able in this experimental setup), and thus can be used asdesign parameters or be determined with covariance adapta-tion method for the estimation performance.

3.2 Load side estimation experiments

The performance validation is conducted for the follow-ing five estimation configurations:

• DKF-AG: Dynamic model (4) based Kalman filter withaccelerometers and gyroscope

• DKF-A: Dynamic model (4) based Kalman filter withaccelerometers only

• KKF-AG: Kinematic model (8) based Kalman filter withaccelerometers and gyroscope

• KKF-A: Kinematic model (8) based Kalman filter withaccelerometers only

• Torsion: Use motor encoder output θm,s as the load sideposition directly. In this case the load side positionestimation error is given by the joint torsion

δ θ θ� �,,

,sm s

sN

= − .

3.2.1 Open loop frequency rich test without covarianceadaption or parametric uncertainty

The effectiveness of the above algorithm configurationsin the open loop frequency rich response is tested first. Themotor torque command is a quadratic chirp signal with themagnitude of 0.2 Nm. The frequency range varies from0.5 Hz to 50 Hz quadratically in 50 sec as shown in Fig. 2.

In this experiment, the algorithms are tested withoutcovariance adaptation or parametric uncertainty to see thegeneral benefits of load side sensors. The fixed noisevariances and the dynamic parameter values used inthe algorithms are listed in Table I. These variances andparameter values are mostly the nominal values identifiedexperimentally as described in Section 3.1, except for those ofnℓf,s, nu, nba, and nbv, which are tuned by trial and error. β in thelow-pass filter in KKF algorithms is selected as 1 (i.e., α = fb).

The estimation errors of the load side position by fiveestimation configurations are shown in Fig. 3. Note that theerrors are of the same increasing frequency as the chirptorque input. In Fig. 3, the rms value on the right top of each

823W. Chen and M. Tomizuka: State Estimation in Elastic Joints

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 7: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

sub-figure denotes the root-mean-square value of the estima-tion error. As expected, the KKF-AG method achieves the bestestimation performance, the rms estimation error of which isless than 1

3of the one by directly using θm (i.e., Torsion). The

utilization of the gyroscope does not benefit the dynamicmodel based algorithms, but results in significant improve-ment for the kinematic model based algorithms. This makessense since the dynamic model (4) uses θm,s, �

�θ ,s , and ���θ ,s as

the model output, all of which are available actual measure-ments, while the kinematic model (8) uses only θℓf,s and �

�θ ,s

as the model output, and θℓf,s is actually approximated byθmf s

N, . With this benefit of the gyroscope in mind, in the

following testing, performance comparisons are conductedonly for configurations where both accelerometers and gyro-scope are available, i.e., DKF-AG and KKF-AG.

3.2.2 Open loop frequency rich test with covarianceadaption and parametric uncertainty

In this experiment, the same open loop frequency richresponse is utilized. The covariance estimation algorithm is

tested with the same initial noise variances listed in Table Iexcept that the variances of nbv and nba are reset to 10−4 and0.1 (different from the tuned suboptimal values). Also,Jℓ and kj are varied with 20% error to check the perfor-mance of DKF when subject to the parametric uncertainty. βin the KKF method is selected as 0.5, i.e., α = 0.5fb. Theadaptive covariance estimation scheme is confined to adaptR̂d only.

Fig. 4 shows that KKF-AG still achieves the best estima-tion performance, where the estimation error is greatlyreduced at all the testing frequencies. DKF-AG doesnot perform as well as KKF-AG, and it amplifies theestimation error at some frequency range due to parametricuncertainties.

The adapted R̂d in both methods is shown in Fig. 5. Asexpected, to match the covariance with the estimation results,R̂d tends to increase when the estimation error is increasing(especially around the resonant frequency 19 Hz at about30 sec).

Fig. 6 shows that both KKF-AG and DKF-AG are effec-tive to estimate the biases of gyroscope and accelerometeroutputs. Thus, the bias effects in the utilization of sensorsignals are attenuated.

In Fig. 6, the small fluctuation effect at the end ofthe trajectory for gyroscope bias estimation is mainly

0 10 20 30 40 500

10

20

30

40

50

60

Fre

quen

cy (

Hz)

Time (sec)

Chirp Input Signal Frequency

Fig. 2. Chirp input signal frequency.

Table I. Nominal parameter values—first experiment (SI units).

var(nu) var(nsm) var(nℓf,s)10−4 8.225 × 10−9 8.225 × 10−9

var(nvℓ) var(naℓ) var(nbv)2.525 × 10−5 3.284 × 10−3 10−5

var(nba) Jm Jℓ

10−2 5.313 × 10−4 6.8kj dj

3.1 × 104 47

0 10 20 30 40 50−2

0

2x 10

−3

rms=0.327 x10−3 rad

Tor

sion

0 10 20 30 40 50−2

0

2x 10

−3

rms=0.116 x10−3 rad

DK

F−

AG

0 10 20 30 40 50−2

0

2x 10

−3

rms=0.116 x10−3 rad

DK

F−

A0 10 20 30 40 50

−2

0

2x 10

−3

rms=0.105 x10−3 rad

KK

F−

AG

0 10 20 30 40 50−2

0

2x 10

−3

rms=0.181 x10−3 rad

KK

F−

A

Time (sec)

Unit: rad

Fig. 3. Estimation errors of load side position in chirpexperiment (without covariance adaptation or parametricuncertainty).

824 Asian Journal of Control, Vol. 16, No. 3, pp. 818–829, May 2014

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 8: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

due to the limited bandwidths of the sensors. The 3 dBbandwidth is 40 Hz for the gyroscope (Analog, ADXRS150)and 2700 Hz for the accelerometer (Kistler, 8330A3).When the chirp frequency increases to around 40 Hz(see Fig. 2) towards the end of the trajectory, the gyroscopemeasurement becomes not as accurate as desired. Thisresults in small fluctuation for the gyroscope bias

estimate in Fig. 6, while the accelerometer bias estimationcontinues similar performance as in the low frequencyregion.

Thanks to the adaptation algorithm for measurementnoise covariances, this fluctuation behavior does not reallyaffect the load side position estimation accuracy (seeFig. 4). Also note that, in Fig. 6, the accelerometer biasestimate by KKF-AG is not as good as DKF-AG at the endpart. This is because that KKF-AG treats the accelerometermeasurement as the control input in the model ratherthan as the measurement output (see (8)). In this paper, weconfined the covariance adaptation to measurement noisesonly. Thus, the accelerometer noise covariance is notadapted in KKF-AG while it is in DKF-AG. This actuallyagain validates the necessity of covariance adaptationin the utilization of such sensors with limitedbandwidth.

On the other hand, the motions in actual robotic tasksare normally at a frequency region much lower than 40 Hz.Therefore, this bias estimation fluctuation will not be aproblem for actual robotic tasks.

3.2.3 Scanning trajectory test with covariance adaption andparametric uncertainty

This experiment is to verify the effectiveness of themethods during a scanning trajectory. The desired load sidetrajectory (Fig. 7) is designed as a fourth-order smooth timeoptimal trajectory suggested in [20]. The controller for tra-jectory tracking is a basic PID feedback controller with amodel based feedforward controller. All the covariances anddynamic parameter values remain the same as those in thesecond chirp experiment (Fig. 4) except for the window size

0 10 20 30 40 50−2

0

2x 10

−3

rms=0.327 x10−3 rad

Tor

sion

0 10 20 30 40 50−2

0

2x 10

−3

rms=0.199 x10−3 rad

DK

F−

AG

0 10 20 30 40 50−2

0

2x 10

−3

rms=0.099 x10−3 rad

Time (sec)

KK

F−

AG

Unit: rad

Fig. 4. Estimation errors of load side position in chirpexperiment (with covariance adaptation and parametricuncertainty).

0 10 20 30 40 500

2.55

x 10−3

DK

F−

AG

R(1

,1)

0 10 20 30 40 5005

1015

x 10−4

DK

F−

AG

R(2

,2)

0 10 20 30 40 5005

1015

DK

F−

AG

R(3

,3)

0 10 20 30 40 500123

x 10−7

KK

F−

AG

R(1

,1)

0 10 20 30 40 500

10

20x 10

−4

Time (sec)

KK

F−

AG

R(2

,2)

Fig. 5. Covariance estimation in chirp experiment (withcovariance adaptation and parametric uncertainty).

0 10 20 30 40 50

0

5

10

15x 10

−3 Bias Estimation of Load Side Gyroscope

Time (sec)

Bia

s (r

ad/s

ec)

0 10 20 30 40 50

0

0.02

0.04

0.06Bias Estimation of Load Side Accelerometer

Time (sec)

Bia

s (r

ad/s

ec2 )

DKF−AGKKF−AGIdentified Bias

DKF−AGKKF−AGIdentified Bias

Fig. 6. Bias estimation in chirp experiment (with covarianceadaptation and parametric uncertainty).

825W. Chen and M. Tomizuka: State Estimation in Elastic Joints

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 9: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

NR used in the covariance adaptation scheme. This experimentis to test the load side estimation only and thus the estimatedload side information is not fed back for control. This con-troller utilizes the motor side encoder feedback and is toachieve the motor side tracking, i.e., θm → θmd, where θmd iscomputed from the desired load side trajectory θℓd (see [1] forcontroller details as well as the utilization of load side esti-mates for feedback control.

The result of this experiment is shown in Fig. 8, whereKKF-AG still achieves the best estimation performance. Italso indicates that the cleanness of the load side positionestimate by KKF-AG is similar to the one by Torsion, whichis computed by the encoder measurements and thus is clean

at the encoder resolution level. Also note that the remainingoscillatory estimation error by KKF-AG is mainly due to thetransmission error [17], which is not considered in the esti-mation algorithm. Fig. 9 illustrates the actual and the esti-mated tracking errors of the load side position. It shows thatthe tracking error estimated by KKF-AG captures mosttrends of the actual tracking error. This indicates the poten-tial of KKF-AG that could benefit the load side positiontracking application. To demonstrate this, the load side posi-tion estimate by KKF-AG has been utilized for real-timefeedback control in the friction compensation in [1]and [15].

IV. DISCUSSION

The above experimental study has demonstrated theadvantages of KKF over DKF. It is noted that the DKF andKKF methods are individually formulated based on differentmodels. However, it may be beneficial to further combineboth the dynamic model and the kinematic model for Kalmanfilter formulation. The following two ways may be used toachieve this objective.

First, in KKF, instead of simple first order low pass filter

G ss

f ( ) =+α

α, the nominal dynamic model

θθ

� �

s

s

d s k

N J s d d s km

j j

j j

( )( )

⎡⎣⎢

⎤⎦⎥

=+

+ + +⎡⎣ ⎤⎦nom

ˆ ˆ

ˆ ( ˆ ˆ ) ˆ2(15)

can be used to obtain the load side position approximationfrom the motor side encoder measurement, where •̂ denotesthe nominal value of •. However, this will result in a higher

0 5 10 15 20 25 300

0.5

1

Time (sec)

Pos

ition

(r

ad)

0 5 10 15 20 25 30−2

0

2

Time (sec)

Vel

ocity

(r

ad/s

ec)

0 5 10 15 20 25 30−5

0

5

Time (sec)

Acc

eler

atio

n (r

ad/s

ec2 )

Fig. 7. Load side desired trajectory.

0 5 10 15 20 25 30−1

0

1x 10

−3

rms=0.384 x10−3 rad

Tor

sion

0 5 10 15 20 25 30−1

0

1x 10

−3

rms=0.215 x10−3 rad

DK

F−

AG

0 5 10 15 20 25 30−1

0

1x 10

−3

rms=0.166 x10−3 rad

Time (sec)

KK

F−

AG

Unit: rad

Fig. 8. Estimation errors of load side position in scanningtrajectory experiment.

0 5 10 15 20 25 30−8

−6

−4

−2

0

2

4

6

8x 10

−4

Time (sec)

Load

Sid

e P

ositi

on

Tra

ckin

g E

rror

Est

imat

ion

(rad

)

Load Side Position Tracking Error Estimation

Using θm,s

Actual ErrorKKF−AG

Fig. 9. Load side actual and estimated position tracking error.

826 Asian Journal of Control, Vol. 16, No. 3, pp. 818–829, May 2014

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 10: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

order model formulation which introduces more complexityand computation load. Actually, it may not be favorable to usea high order model since its high frequency response isalways subject to large errors due to model uncertainties.Furthermore, one can note that nonlinear dynamic terms suchas Coulomb friction and transmission error are still notconsidered in this method due to the utilization of the lineartransfer function.

The second way to approximate the load side positionmeasurement is to use the dynamic model (2) directly, whichyields

θ θ θ

θ

��

��

, , ,

,

s

j j

jm s

jm s

s m m m s m

d s k

k

N

d

N

N u f J d

=+

+⎡

⎣⎢

− + − −

1ˆ ˆ

ˆ ˆ

ˆ ˆ ˆ ˆ ��θm s,( )⎤⎦(16)

where θm,s and �θm s, are obtained from motor encodermeasurements. us can be either motor torque command ormeasured by motor current. The desired trajectory ��θmd can beused instead of ��̂

,θm s in (16) as approximation. By thisapproach, the nonlinear dynamic terms (e.g., Coulombfriction and transmission error) can be taken into accountas a lumped term f̂m if a good model knowledge isavailable. This first order model formulation is relativelyeasy and efficient to realize. This extension has been adoptedin [21] for the load side state estimation in the multi-jointrobot.

V. CONCLUSIONS

This paper investigated the estimation algorithms forload side information of robotic elastic joints with a multi-sensor configuration. Kalman filtering methods were formu-lated based on the extended dynamic/kinematic modelstaking measurement dynamics (bias and noise) into consid-eration. Noise covariance adaptation was also studied. Theeffectiveness of the methods was verified experimentally on asingle-DOF elastic joint testbed. The benefits of load sidesensing and the advantages of KKF over DKF have beendemonstrated.

VI. APPENDIX A

6.1 COVARIANCE ADAPTATION

Consider the discrete time model (9) with theassumption that the initial condition x Xd d1 1( ) ( ) =∼

x Mdo

d1 1( ) ( )( )N ˆ , , the process noise w k W kd d( ) ( )∼ =Q kd( , ( ))N 0 , and the measurement noise v k V kd d( ) ( )∼ =

R kd( , ( ))N 0 . The Kalman filter to estimate the system statesis given by (10)–(11). If noise covariances Qd(k) and Rd(k) areunknown, the real-time estimation ((12) and (14)) for Qd(k)and Rd(k) can be derived based on the maximum likelihoodprinciple [19,18]. Furthermore, if noise covariances Qd and Rd

are time invariant constants, expectation maximizationmethod [22,23] can be well applied to iteratively estimatethese covariances in an off-line manner. Here, however, sincethe covariances of fictitious noises (Qd and Rd) are likely to betime varying depending on the trajectory, we want to estimatethem online. Therefore, only sub-optimal maximum likeli-hood solutions can be derived to estimate Qd and Rd sepa-rately. There are other adaptive filtering methods introducedin [19,18], such as Bayesian methods, correlation methods,and covariance matching. Among these methods, covariancematching typically requires the least computation and storagealthough the convergence property may be subject to questionin certain cases. It is, however, worth pointing out that thesame sub-optimal maximum likelihood solutions for thisproblem can be derived by the covariance matching techniqueas detailed below.

Note that, if Qd(k) and Rd(k) are known constants, theKalman filter yields the optimal estimation for this problem.It follows that

E x k x k E L k y k y k L k

L k M k

d d d do

do

d

d d d

Δ Δ( ) ( ) ( ) ( ) ( ) ( )

( ) (

T T T[ ] = [ ]=

� �

C )) ( ) ( )

( ) ( ) ( ) ( ) ( )

C

C C C

d d d

d d d d d d d d

R k L k

L k M k L k Z k L k

T T

T T T T

si

+( )= +

nnce T

T T

L k Z k R k

L k M k L k

I L k

d d d d

d d d d d

d

( ) ( ) ( )

( ) ( ) ( )

( )

=( )=

+ −

−C

C C

C

1

dd d d d

d d d d d d

M k L k

M k L k L k M k

( )= =

( ) ( )

( ) ( ) ( ) ( )

C

C C

T T

T T (17)

Hereafter the conditional expectation E[•|j] given theinformation up to the j-th time step is denoted as E[•] forsimplicity.

Thus,

Z k M k L k M k

M k E x k x k

M k E x

d d d d d

d d d

d d

( ) ( ) ( ) ( )

( ) ( ) ( )

( )

= −= − [ ]

⇒ =

C

Δ ΔΔ

T

(( ) ( ) ( )

( ) ( ) ( ), ,

k x k Z k

M k Z k Q k

d d

d d d d d w d d w

Δ T

T TAlso

[ ]+= − +

⇒A A B B

B

1

dd w d d w d d d

d d d

Q k E x k x k Z k

Z k

, ,( ) ( ) ( ) ( )

( )

B

A A

T T

T

= [ ]+− −

Δ Δ1

(18)

This means that the theoretical value for Qd(k)should match the actual estimation residual covarianceE x k x kd dΔ Δ( ) ( )T[ ] as in (18).

Similarly, the theoretical matching value for Rd(k) canbe obtained. Note that,

827W. Chen and M. Tomizuka: State Estimation in Elastic Joints

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 11: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

E y k y k E x k x k v k

x k x k

d d d d d d

d d d

Δ Δ( ) ( )[ ] = ( ) − ( )( ) + ( )( )⎡⎣⋅ ( ) − (

T C

C

ˆ

ˆ ))( ) + ( )( ) ⎤⎦= ( ) − ( )( ) ( ) − ( )( )⎡⎣

+

v k

E x k x k x k x k

x

d

d d d d d d

d d

T

T TC C

C

ˆ ˆ

kk x k v k

v k x k x k v k v k

d d

d d d d d d

( ) − ( )( ) ( )+ ( ) ( ) − ( )( ) + ( ) ( )⎤⎦

=

ˆ

ˆ

T

T T TC

CC C Cd d dT

d d d d

d d d

Z k E x k x k v k

E v k x k x k

( ) + ( ) − ( )( ) ( )[ ]+ ( ) ( ) − ( )( )

ˆ

ˆ

T

TT TCd dR k⎡⎣ ⎤⎦ + ( )(19)

Furthermore,

E x k x k v k

E x k x k L k y k x

d d d d

d d do

d d d d

C

C C

( ) − ( )( ) ( )[ ]= ( ) − ( ) − ( ) ( ) −

ˆ

ˆ

T

ood

d d d d do

d d

k v k

E I L k x k x k L k v k

( )( )( ) ( )[ ]= − ( )( ) ( ) − ( )( ) − ( ) ( )

T

C C ˆ(( ) ( )[ ]= − ( ) ( ) ( )[ ] = − ( ) ( )= − ( )

v k

E L k v k v k L k R k

Z k

d

d d d d d d d

d d

T

TC C

C Cdd d d d dL k Z k R kT Tsince ( ) = ( ) ( )( )−C 1

Therefore, (19) is further reduced to

E y k y k Z k Z k R k

R k E y k

d d d d d d d d d

d d

Δ ΔΔ

( ) ( ) ( ) ( ) ( )

( ) (

T T T[ ] = − +⇒ =

C C C C2

)) ( ) ( )Δy k Z kd d d dT T[ ]+ C C

(20)

For estimation based on the above covariance matchinganalysis, the expectations in (18) and (20) can be approxi-mated by the average over the most recent Ns samples. An adhoc way to reduce the filter memory requirement is byreplacing the expectation with the most recent data only(i.e., Ns = 1) and then implementing the update in an expo-nential moving average form. This leads to the solution in(12) and (14).

REFERENCES

1. Chen, W., K. Kong, and M. Tomizuka, “Hybrid AdaptiveFriction Compensation of Indirect Drive Trains,” Proc.2009 ASME Dyn. Syst. Control Conf., Hollywood,California, pp. 313–320 (2009).

2. Spong, M. W., “Modeling and control of elastic jointrobots,” J. Dyn. Syst. Meas. Control Trans. ASME, Vol.109, No. 4, pp. 310–318 (1987).

3. Tomei, P., “A simple pd controller for robots with elasticjoints,” IEEE Trans. Autom. Control, Vol. 36, pp. 1208–1213 (1991).

4. Ailon, A. and R. Ortega, “An observer-based set-pointcontroller for robot manipulators with flexible joints,”Syst. Control Lett., Vol. 21, No. 4, pp. 329–335,(1993).

5. Zhao, S. and Z. Gao, “An active disturbance rejectionbased approach to vibration suppression in two-inertiasystems,” Asian J. Control, Vol. 15, No. 2, pp. 350–362(2013).

6. Feliu, V., F. Castillo, V. Jaramillo, and G. Partida, “Arobust controller for a 3-dof flexible robot with a timevariant payload,” Asian J. Control, Vol. 15, No. 4, pp.1–12 (2013).

7. Hu, J.-S., F.-R. Hu, and C.-H. Kang, “On the two-inertiasystem: Analysis of the asymptotic behaviors to multiplefeedback position control,” Asian J. Control, Vol. 15, No.6, pp. 1–13 (2013).

8. Roan, P., N. Deshpande, Y. Wang, and B. Pitzer,“Manipulator state estimation with low cost accelerom-eters and gyroscopes,” In Intelligent Robots and Systems(IROS), 2012 IEEE/RSJ International Conference on,pp. 4822–4827 (2012).

9. Jankovic, M., “Observer based control for elastic jointrobots,” IEEE Trans. Robot. Autom., Vol. 11, pp. 618–623(1995).

10. De Luca, A., D. Schroder, and M. Thummel, “Anacceleration-based state observer for robot manipulatorswith elastic joints,” Robot. Autom. 2007 IEEE Int. Conf.,Rome, Italy, pp. 3817–3823 (2007).

11. Talole, S., J. Kolhe, and S. Phadke, “Extended-state-observer-based control of flexible-joint system withexperimental validation,” IEEE Trans. Ind. Electron., Vol.57, pp. 1411–1419 (2010).

12. Ulrich, S. and J. Sasiadek, “Extended kalman filtering forflexible joint space robot control,” Proc. 2001 Am.Control Conf., San Francisco, California, pp. 1021–1026(2011).

13. Lee, D. J. and M. Tomizuka., “State/ parameter/ distur-bance estimation with an accelerometer in precisionmotion control of a linear motor,” Proc. 2001 ASME Int.Mechatronics Eng. Congr. Expo., New York, pp. 11–16(2001).

14. Jeon, S. and M. Tomizuka, “Benefits of accelerationmeasurement in velocity estimation and motion control,”Control Eng. Practice,Vol. 15, No. 3, pp. 325–332 (2007).

15. Chen, W. and M. Tomizuka, “Estimation of load sideposition in indirect drive robots by sensor fusion andkalman filtering,” Proc. 2010 Amer. Control Conf., Bal-timore, Maryland, pp. 6852–6857 (2010).

16. Franklin, G., J. Powell, and M. Workman, Digital Controlof Dynamic Systems. Addison-Wesley, Boston, MA,second ed., (1990).

17. Ghorbel, F. H., P. S. Gandhi, and F. Alpeter, “On thekinematic error in harmonic drive gears,” J. Mech. Des.,Vol. 123, No. 1, pp. 90–97 (2001).

18. Mehra, R., “Approaches to adaptive filtering,” IEEETrans. Autom. Control, Vol. 17, No. 5, pp. 693–698(1972).

828 Asian Journal of Control, Vol. 16, No. 3, pp. 818–829, May 2014

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd

Page 12: COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTSwjchen84.github.io/publications/J2014_ASJC.pdf · COMPARATIVE STUDY ON STATE ESTIMATION IN ELASTIC JOINTS Wenjie Chen and Masayoshi

19. Maybeck, P. S., Stochastic Models, Estimation, andControl, Vol. 2. Academic Press, Inc., New York (1982).

20. Lambrechts, P., M. Boerlage, and M. Steinbuch, “Trajec-tory planning and feedforward design for electrome-chanical motion systems,” Control Eng. Practice,Vol. 13, No. 2, pp. 145–157 (2005).

21. Chen, W. and M. Tomizuka, “Load side state estimationin robot with joint elasticity,” Proc. IEEE/ASME Int.Conf. Adv. Intell. Mechatronics, Kaohsiung, Taiwan,pp. 598–603 (2012).

22. Shumway, R. H. and D. S. Stoffer, “An approach to timeseries smoothing and forecasting using the em algo-rithm,” J. Time Ser. Anal., Vol. 3, pp. 253–264 (1982).

23. Ghahramani, Z. and G. E. Hinton, “Parameter estimationfor linear dynamical systems,” Tech. Rep. CRG-TR-96-2,University of Toronto (1996).

Wenjie Chen received B.Eng. degree fromZhejiang University, Zhejiang, China, in2007, and the M.S. and Ph.D. degrees fromUniversity of California, Berkeley, CA,USA, in 2009 and 2012, respectively, all inMechanical Engineering.

Dr. Chen is currently PostdoctoralScholar in Department of Mechanical Engineering at the Uni-versity of California, Berkeley. His research interests includedesign and implementation of advanced control and sensingalgorithms with applications to robotic/mechatronic systems,such as industrial robots, wearable assistive robotics, androbots for advanced manufacturing.

Masayoshi Tomizuka received B.S. andM.S. degrees in Mechanical Engineeringfrom Keio University, Tokyo, Japan, andPh.D. degree in Mechanical Engineeringfrom Massachusetts Institute of Technol-ogy (MIT), Cambridge, MA, USA, in1974.

Dr. Tomizuka joined Department of Mechanical Engi-neering, University of California, Berkeley, in 1974, where heis currently the Cheryl and John Neerhout, Jr., DistinguishedProfessor. He teaches courses on dynamic systems and con-trols and conducts research on optimal and adaptive control,digital control, motion control, and their applications torobotics, manufacturing, information storage devices, andvehicles.

Dr. Tomizuka served as Program Director of DynamicSystems and Control Program of the Civil and MechanicalSystems Division of National Science Foundation (2002–2004). He was Technical Editor of ASME Journal of DynamicSystems, Measurement and Control (1988–1993), and Editor-in-Chief of IEEE/ASME Transactions on Mechatronics(1997–1999). He was the recipient of Rudolf Kalman BestPaper Award (ASME, 1995, 2010), DSCD Outstanding Inves-tigator Award (ASME, 1996), Charles Russ Richards Memo-rial Award (ASME, 1997), Rufus Oldenburger Medal(ASME, 2002), and John R. Ragazzini Award (AmericanAutomatic Control Council, 2006). He is Fellow of AmericanSociety of Mechanical Engineers (ASME), the Institute ofElectrical and Electronics Engineers (IEEE), the InternationalFederation of Automatic Control (IFAC), and the Society ofManufacturing Engineers (SME).

829W. Chen and M. Tomizuka: State Estimation in Elastic Joints

© 2013 Chinese Automatic Control Society and Wiley Publishing Asia Pty Ltd