Table of Contents Author Guidelines Submit a Manuscript
Discrete Dynamics in Nature and Society
Volume 2010, Article ID 482972, 14 pages
Research Article

Extended and Unscented Kalman Filtering Applied to a Flexible-Joint Robot with Jerk Estimation

1Control Engineering Department, Faculty of Electrical and Computer Engineering, University of Tabriz, Tabriz 5166614776, Iran
2Department of Mechanical and Industrial Engineering, Concordia University, Montreal, QC, Canada H3G 1M8

Received 26 July 2010; Accepted 8 November 2010

Academic Editor: Recai Kilic

Copyright © 2010 Mohammad Ali Badamchizadeh et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.


Robust nonlinear control of flexible-joint robots requires that the link position, velocity, acceleration, and jerk be available. In this paper, we derive the dynamic model of a nonlinear flexible-joint robot based on the governing Euler-Lagrange equations and propose extended and unscented Kalman filters to estimate the link acceleration and jerk from position and velocity measurements. Both observers are designed for the same model and run with the same covariance matrices under the same initial conditions. A five-bar linkage robot with revolute flexible joints is considered as a case study. Simulation results verify the effectiveness of the proposed filters.

1. Introduction

In recent years, many researchers have investigated the control problem of robots with flexible joints. However, compared to the large volume of the literature available on control of rigid robots, relatively little has been published on the control of flexible-joint robots. On the other hand, for a robot manipulator to carry out demanding tasks with high performance, such as the space robots performing services in space, joint flexibility due to gear elasticity, shaft windup and use of harmonic drives, has to be taken into account in both modeling and control of robot manipulators. As the experimental investigations by Sweet and Good [1] show, the effects of joint flexibility can limit the robustness and performance of a given robot controller and can even lead to instability if neglected in the controller design. Moreover, the joint flexibility can serve as the first approximation of robot link flexibility [2] and hence the study of joint flexibility can provide another perspective into the study of flexible link which allows much lighter link weight and thus much faster motion of the robot. If we assume that the flexibility is modeled as a linear torsional spring, we obtain the dynamic model of the manipulator with flexible joints. Flexible robotic manipulators have several advantages over rigid manipulators depending on specific applications; some of these advantages are as follows: low actuator drive requirement, high speeds, low weight, small size, and low cost [36].

The robust nonlinear control of these robots based on the state feedback requires the knowledge of state variables for each joint, which may be either positions or velocities of the motors and of the links or positions, velocities, accelerations and jerks of the links [5]. Not only is the full state measurement costly, but there are problems with link acceleration and jerk. Since the former is difficult and the latter impossible to measure, we are forced to consider an observer which provides estimations for these unmeasurable states from position and velocity measurements [7].

Within the significant toolbox of mathematical tools that can be used for stochastic estimation from noisy sensor measurements, one of the most well-known and often used tools is the Kalman filter. As an extension to the same idea, the extended Kalman filter (EKF) is used if the dynamic of the system and/or the output dynamic is nonlinear. It is based on linearization about the current estimation error mean and covariance [8]. Although it is straightforward and simple, it has drawbacks too [9]. The Unscented Kalman Filter (UKF) is the newest revision of the Kalman Filter, proposed to overcome these flaws. It does not need the linearization for a nonlinear function and is more accurate and simpler than the EKF applied to nonlinear systems [10, 11].

In this paper, we derive the dynamic model of a five-bar linkage robot with flexible joints and propose a state-space model for the robot. Then, we apply extended and unscented Kalman filters to estimate the proposed state for the robot. The augmented state is herein composed of the position, velocity, acceleration and jerk of the links. Computer simulations are well performed to verify the performances of the proposed filters.

The remainder of this paper is organized as follows. Section 2 presents the dynamic model of a five-bar linkage robot with flexible joints and derives a state-space model for the robot as well. In Sections 3 and 4, we describe the algorithmic details of the EKF and UKF formulations and implement these filters for the five-bar linkage robot. Section 5 shows the simulation results and discusses their significance. And Section 6 gives the concluding remarks.

2. Dynamic Modeling

Referring to Figure 1, we define 𝑞1=[𝑞𝑙1,,𝑞𝑙𝑛]𝑇 as the vector of link angles and 𝑞2=[(1/𝑟𝑚1)𝑞𝑚1,,(1/𝑟𝑚𝑛)𝑞𝑚𝑛]𝑇 as the vector of motor shaft angles (reflected to the link side of the gears) for the 𝑛-link flexible-joint manipulator. The dynamic model can be derived using the Euler-Lagrange equations [7, 12]𝐷𝑞1̈𝑞1𝑞+𝐶1,̇𝑞1̇𝑞1𝑞+𝑔1𝑞+𝐾1𝑞2=0,𝐽̈𝑞2+𝐵̇𝑞2𝑞+𝐾2𝑞1=𝑢,(2.1) where 𝐷(𝑞1) is the inertia matrix (symmetric and positive definite), 𝐶(𝑞1,̇𝑞1) is the vector representing the damping, Coriolis and centrifugal torques, 𝑔(𝑞1) is the vector of torques due to gravity, 𝐾=diag[𝑘1,,𝑘𝑛] is the joint stiffness coefficients modeling the joints elasticity, 𝐽=diag[𝐽1,,𝐽𝑛] and 𝐵=diag[𝐵1,,𝐵𝑛] are diagonal matrices representing rotor inertia and rotor damping, respectively, and 𝑢𝑅𝑛 is the vector of input torques applied to the rotor [7, 12].

Figure 1: Model of a single-link, flexible-joint manipulator.

Figure 2, shows the five-bar linkage manipulator with flexible joints built in robotics research lab in our department. Also, Figure 3 depicts the 5-bar linkage manipulator schematic where the links form a parallelogram [13]. It is clear from this figure that even though there are four links being moved, there are in fact only two degrees of freedom, identified as 𝑞𝑙1 and 𝑞𝑙2 [12].

Figure 2: Five-bar linkage manipulator.
Figure 3: Planar schematic of 5-bar linkage robot.

We adopt a similar approach, that is, successive differentiation of the link position with respect to time, introduced by [7] to derive a state-space model for the 5-bar linkage robot. So the state vector is defined as 𝑞𝑥=1𝑞2̇𝑞1̇𝑞2̈𝑞1𝑞1𝑇,(2.2) where 𝑞1=𝑞𝑙1𝑞𝑙2𝑇,𝑞2=1𝑟𝑚1𝑞𝑚11𝑟𝑚2𝑞𝑚2𝑇.(2.3) Then following the discussion in [12], we set 𝑚3𝑙2𝑙𝑐3=𝑚4𝑙1𝑙𝑐4,(2.4) which subsequently makes 𝑑12 and 𝑑21 be zero, that is, the inertia matrix becomes diagonal and constant. Therefore using (2.1)–(2.4), the rather complex-looking manipulator in Figure 2 can be expressed by the following decoupled set of equations:̇𝑥1=𝑥5,̇𝑥2=𝑥6,̇𝑥3=𝑥7,̇𝑥4=𝑥8,̇𝑥5=1𝑑11𝑔cos𝑥1𝑚1𝑙𝑐1+𝑚3𝑙𝑐3+𝑚4𝑙1𝑘1𝑥1𝑥3,̇𝑥6=1𝑑22𝑔cos𝑥2𝑚2𝑙𝑐2𝑚4𝑙𝑐4+𝑚3𝑙2𝑘1𝑥2𝑥4,̇𝑥7=1𝐽1𝑢1𝐵1𝑥7𝑘1𝑥3𝑥1,̇𝑥8=1𝐽2𝑢2𝐵2𝑥8𝑘2𝑥4𝑥2,̇𝑥9=1𝑑11𝑔𝑥5sin𝑥1𝑚1𝑙𝑐1+𝑚3𝑙𝑐3+𝑚4𝑙1𝑘1𝑥5𝑥7,̇𝑥10=1𝑑22𝑔𝑥6sin𝑥2𝑚2𝑙𝑐2𝑚4𝑙𝑐4+𝑚3𝑙2𝑘2𝑥6𝑥8,̇𝑥11=1𝑑11𝑔̇𝑥5sin𝑥1𝑚1𝑙𝑐1+𝑚3𝑙𝑐3+𝑚4𝑙1+𝑔𝑥25cos𝑥1×𝑚1𝑙𝑐1+𝑚3𝑙𝑐3+𝑚4𝑙1𝑘1̇𝑥5̇𝑥7,̇𝑥12=1𝑑22𝑔̇𝑥6sin𝑥2𝑚2𝑙𝑐2𝑚4𝑙𝑐4+𝑚3𝑙2+𝑔𝑥26cos𝑥2×𝑚2𝑙𝑐2𝑚4𝑙𝑐4+𝑚3𝑙2𝑘2̇𝑥6̇𝑥8.(2.5) This special feature helps to explain the popularity of the parallelogram configuration in industrial robots; since one can adjust the two set of angles (𝑞𝑙1,𝑞𝑚1) and (𝑞𝑙2,𝑞𝑚2) independently, without worrying about interactions between them.

3. Extended Kalman Filter

The Kalman filter addresses the general problem of trying to estimate the state 𝑥𝑅𝑛 of a discrete-time controlled process that is governed by a linear stochastic difference equation. As an extension to the same idea, the extended Kalman filter (EKF) is used if the dynamic of the system and/or the output dynamic is nonlinear. EKF is based on linearization about the current estimation error mean and covariance [8].

3.1. Definitions

Let us assume that the process has a state vector 𝑥𝑅𝑛 and a control vector 𝑢 and is governed by the nonlinear stochastic difference equation 𝑥𝑘𝑥=𝑓𝑘1,𝑢𝑘,𝑤𝑘1,(3.1) with a measurement 𝑧𝑅𝑚that is 𝑧𝑘𝑥=𝑘,𝑣𝑘,(3.2) the random variables 𝑤𝑘 and 𝑣𝑘represent the process and measurement noise, respectively. They are assumed to be independent of each other, white, and with normal probability distributions with covariance matrices 𝑄 and 𝑅. It can be shown that the time update equations of EKF is ̂𝑥𝑘=𝑓̂𝑥𝑘1,𝑢𝑘,𝑃,0𝑘=𝐴𝑘𝑃𝑘1𝐴𝑇𝑘+𝑊𝑘𝑄𝑘1𝑊𝑇𝑘,(3.3) where ̂𝑥𝑘is the a priori state estimate [8]. These time update equations project the state and covariance estimate (𝑃𝑘) from the previous time step 𝑘1to the current time step𝑘. And the measurement update equations of the EKF are 𝐾𝑘=𝑃𝑘𝐻𝑇𝑘𝐻𝑘𝑃𝑘𝐻𝑇𝑘+𝑉𝑘𝑅𝑘𝑉𝑇𝑘1,̂𝑥𝑘=̂𝑥𝑘+𝐾𝑘𝑧𝑘̂𝑥𝑘,𝑃,0𝑘=𝐼𝐾𝑘𝐻𝑘𝑃𝑘,(3.4) where 𝐴,𝑊,𝐻and 𝑉are Jacobian matrices and 𝐾is the correction gain vector. These measurement update equations correct the state and covariance estimate with the measurement 𝑧𝑘 [8]. The design process of this filter is explained next.

3.2. Implementation

The differential equations are integrated using a fourth-order Runge Kutta method with a step size of 14 msec.

Suppose that the position and velocity of the 5-bar linkage robot are measured as 𝑧𝑘=𝑞𝑙1𝑞𝑙2̇𝑞𝑙1̇𝑞𝑙2𝑘+𝑣𝑘,(3.5) where 𝑣𝑘represents the measurement noise.

Due to the recursive nature of the EKF algorithm, the state vector needs to be initialized in startup. The initial position and velocity components are taken as the first measured values. Here, the following initial conditions are selected randomly for the state vector:𝑥initial=𝜋2𝜋𝜋4𝜋200000000𝑇.(3.6) We add uncertainty to the initial condition by selecting 𝑃0=3𝜋18023𝜋18022𝜋18022𝜋180211111111𝑇,(3.7) and the process noise and measurement noise are chosen as𝑄=diag3𝜋18023𝜋18022𝜋18022𝜋1802,1010202030304040𝑅=diag5𝜋18025𝜋1802.44(3.8)

Thus, we developed all the necessary elements of the EKF. In Section 5 the results of simulating the filter are presented.

4. Unscented Kalman Filter

The basic premise behind the unscented Kalman filter is based on the idea that it is easier to approximate a Gaussian distribution than it is to approximate an arbitrary nonlinear function. Instead of linearizing using Jacobian matrices, the UKF uses a deterministic sampling approach to capture the mean and covariance estimates with a minimal set of sample points, and it has 3rd-order (Taylor series expansion) accuracy for Gaussian error distribution for any nonlinear system [11], while EKF uses linearizing Jacobian matrix, which is a first-order approximation. The UKF is claimed to have obvious advantages over EKF [10]. A brief overview of the UKF algorithm is presented in the following section.

4.1. Definitions

The unscented transformation (UT) is a method for calculating the statistics of a random variable which undergoes a nonlinear transformation. The 𝐿 dimensional random variable 𝑥 with mean 𝑥and covariance 𝑃𝑥is approximated by 2𝐿+1 weighted points given by𝜒0=𝜒𝑥,𝑖=0,𝑖=𝑥+(𝐿+𝜆)𝑃𝑥𝑖𝜒,𝑖=1,,𝐿,𝑖=𝑥(𝐿+𝜆)𝑃𝑥𝑖𝐿,𝑖=𝐿+1,,2𝐿.(4.1) These sigma points are propagated through the nonlinear function 𝑦𝑖𝜒=𝑓𝑖,𝑖=0,,2𝐿,(4.2) from which the mean and covariance of the transformed probability can be approximated,𝑦2𝐿𝑖=0𝑊𝑚𝑖𝑦𝑖,𝑃𝑦2𝐿𝑖=0𝑊𝑐𝑖𝑦𝑖𝑦𝑦𝑖𝑦𝑇,(4.3) with weights 𝑊𝑖 given by𝑊𝑚0=𝜆,𝑊𝐿+𝜆𝑐0=𝜆+𝐿+𝜆1𝛼2,𝑊+𝛽𝑚𝑖=𝑊𝑐𝑖=12(𝐿+𝜆),𝑖=1,,2𝐿,(4.4) where 𝜆=𝛼2(𝐿+𝜅)𝐿 is a scaling parameter. The constant 𝛼 determines the spread of the sigma points around 𝑥 and is usually set to small positive values less than one (typically in the range 0.001 to 1) whereas 𝜅 is the secondary scaling parameter usually set to zero or 3𝐿, and the constant 𝛽 is used to incorporate prior knowledge of the distribution of 𝑥 (for Gaussian distributions, 𝛽=2 is optimal). The scale parameters may be tuned to match the specific problem; some guidelines to choose them are provided in [10].

The unscented Kalman filter (UKF) can be implemented using UT by expanding the state space to include the noise component:𝑥𝑎𝑘=[𝑥𝑇𝑘,𝑤𝑇𝑘,𝑣𝑇𝑘]𝑇. The UKF algorithm can be summarized as follows:(1)initialization: ̂𝑥𝑎0=̂𝑥𝑇000𝑇,𝑃𝑎0=𝑃0.000𝑄000𝑅(4.5)(2)iteration for each time step 𝑘(1,,),(a)calculate sigma-points: 𝜒𝑎𝑘1=̂𝑥𝑎𝑘1̂𝑥𝑎𝑘1+𝛾𝑃𝑎𝑘1̂𝑥𝑎𝑘1𝛾𝑃𝑎𝑘1,(4.6)(b)time update: 𝜒𝑥𝑘|𝑘1𝜒=𝑓𝑥𝑘1,𝜒𝑤𝑘1,𝑢𝑘1,̂𝑥𝑘=2𝐿𝑖=0𝑊𝑚𝑖𝜒𝑥𝑖,𝑘𝑘1,𝑃𝑘=2𝐿𝑖=0𝑊𝑐𝑖𝜒𝑥𝑖,𝑘𝑘1̂𝑥𝑘𝜒𝑥𝑖,𝑘𝑘1̂𝑥𝑘𝑇,𝒵𝑘𝑘1𝜒=𝑥𝑘𝑘1,𝜒𝑣𝑘1,̂𝑧𝑘=2𝐿𝑖=0𝑊𝑚𝑖𝒵𝑖,𝑘𝑘1;(4.7)(c)measurement update: 𝑃̂𝑧𝑘̂𝑧𝑘=2𝐿𝑖=0𝑊𝑐𝑖𝒵𝑖,𝑘𝑘1̂𝑧𝑘𝒵𝑖,𝑘𝑘1̂𝑧𝑘𝑇,𝑃̂𝑥𝑘̂𝑧𝑘=2𝐿𝑖=0𝑊𝑐𝑖𝜒𝑥𝑖,𝑘𝑘1̂𝑥𝑘𝒵𝑖,𝑘𝑘1̂𝑧𝑘𝑇,𝐾𝑘=𝑃̂𝑥𝑘̂𝑧𝑘𝑃1̂𝑧𝑘̂𝑧𝑘,̂𝑥𝑘=̂𝑥𝑘+𝐾𝑘𝑧𝑘̂𝑧𝑘,𝑃𝑘=𝑃𝑘𝐾𝑘𝑃̂𝑧𝑘̂𝑧𝑘𝐾𝑇𝑘,(4.8) where 𝜒𝑎=(𝜒𝑥)𝑇(𝜒𝑤)𝑇(𝜒𝑣)𝑇𝑇,𝛾=𝐿+𝜆.(4.9)

4.2. Implementation

The differential equations are integrated using a fourth-order Runge Kutta method with a step size of 14 msec. We initialize the filter in the same way as the EKF, using the same values for the state vector and covariance matrices. We also need to set the tuning parameters𝛼, 𝛽 and 𝜅. The optimum values for coefficients 𝛼 and 𝛽are chosen as 0.001 and 2, respectively. And 𝜅 is set to zero. These optimum values are chosen such that they provide the best estimates for all experiments [11].

5. Simulation Results

This section presents simulation results by Matlab. The simulation data and nominal values of the five-bar linkage parameters are selected as shown in Tables 1 and 2. Also, the simulation step time is chosen 14 msec. To evaluate the performances of the proposed EKF and UKF for the five-bar linkage robot, we plotted the estimated states from the available measurements in Figures 4, 5, 6, and 7. They belong to the the first joint. The results for the second joint are similar.

Table 1: 5-Bar linkage manipulator data.
Table 2: Simulation data for 5-bar linkage.
Figure 4: The estimated position 𝑞𝑙1.
Figure 5: The estimated position 𝑞𝑚1.
Figure 6: The estimated velocity ̇𝑞𝑙1.
Figure 7: The estimated velocity ̇𝑞𝑚1.

Moreover, Figures 8, 9, 10, and 11, depict the estimated accelerations and jerks for both of the joints.

Figure 8: The estimated acceleration ̈𝑞𝑙1 of the first joint.
Figure 9: The estimated acceleration ̈𝑞𝑙2 of the second joint.
Figure 10: The estimated jerk 𝑞𝑙1 of the first joint.
Figure 11: The estimated jerk 𝑞𝑙2 of the second joint.

6. Conclusion

In this paper the extended and unscented Kalman filters are employed for state estimation of a flexible-joint robot. First, the dynamic model of a five-bar manipulator is derived in order to apply the proposed filters. Then, simulation results illustrate that both filters can estimate the link acceleration and provide estimates of the link jerk using the position and velocity measurements. Knowledge of these states is necessary for application of robust outer-loop control theory. In the future we will try to utilize these estimations in robust nonlinear tracking controller design for flexible-joint robots. Furthermore, developing system identification techniques for the five-bar linkage robot would be another challenging task.


The authors would like to thanks Ms. Somayyeh Nalan for her assistance.


  1. L. M. Sweet and M. C. Good, “Redefinition of the robot motion control problem: Effects of plant dynamics, drive system constraints, and user requirements,” in Proceedings of the 23rd IEEE Conference on Decision and Control, pp. 724–732, Las Vegas, Nev, USA, 1984. View at Scopus
  2. H. Asada, Z. D. Ma, and H. Tokumaru, “Inverse dynamics of flexible robot arms. Modeling and computation for trajectory control,” Journal of Dynamic Systems, Measurement and Control, vol. 112, no. 2, pp. 177–185, 1990. View at Google Scholar · View at Scopus
  3. M. C. Chien and A. C. Huang, “Adaptive control for flexible-joint electrically driven robot with time-varying uncertainties,” IEEE Transactions on Industrial Electronics, vol. 54, no. 2, pp. 1032–1038, 2007. View at Publisher · View at Google Scholar · View at Scopus
  4. N. Lechevin and P. Sicard, “Observer design for flexible joint manipulators with parameter uncertainties,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '97), vol. 3, pp. 2547–2552, Albuquerque, NM, USA, April 1997. View at Scopus
  5. P. Tomei, “An observer for flexible joint robots,” IEEE Transactions on Automatic Control, vol. 35, no. 6, pp. 739–743, 1990. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at Scopus
  6. I. Hassanzadeh, H. Kharrati, and J.-R. Bonab, “Model following adaptive control for a robot with flexible joints,” in Proceedings of the IEEE Canadian Conference on Electrical and Computer Engineering (CCECE '08), pp. 1467–1472, Niagara Falls, Canada, May 2008. View at Publisher · View at Google Scholar
  7. S. A. Bortoff, J. Y. Hung, and M. W. Spong, “Discrete-time observer for flexible-joint manipulators,” in Proceedings of the 28th IEEE Conference on Decision and Control, vol. 3, pp. 2078–2082, Tampa, Fla, USA, December 1989. View at Scopus
  8. G. Welch and G. Bishop, “An introduction to the Kalman filter,” in Proceedings of the Annual Conference on Computer Graphics & Interactive Techniques (SIGGRAPH '01), Los Angeles, Calif, USA, August 2001.
  9. S. Julier, J. Uhlmann, and H. F. Durrant-Whyte, “A new method for the nonlinear transformation of means and covariances in filters and estimators,” IEEE Transactions on Automatic Control, vol. 45, no. 3, pp. 477–482, 2000. View at Google Scholar · View at Zentralblatt MATH · View at Scopus
  10. S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004. View at Publisher · View at Google Scholar · View at Scopus
  11. E. A. Wan and R. van der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proceedings of IEEE Symposium on Adaptive Systems for Signal Processing, Communication and Control (AS-SPCC '00), pp. 153–158, Lake Louise, Canada, October 2000.
  12. M. W. Spong and M. Vidyasagar, Robot Dynamics and Control, John Wiley & Sons, New York, NY, USA, 2006.
  13. P. Huissoon and D. Wang, “On the design of a direct drive 5-bar-linkage manipulator,” Robotica, vol. 9, no. 4, pp. 441–446, 1991. View at Google Scholar · View at Scopus