### Performance Monitoring and Control of Robotic Manipulators Using Kalman-Filter Approach

**Attila L. Bencsik, IMRE RUDAS**

Budapest Tech

H-1034 Budapest,Doberdo út 6.

HUNGARY

Abstract – This paper addresses the problem to design control and performance monitoring systems for robotic manipulators. The control scheme is a modification of the computed torque method where the input torques necessary to drive the robotic manipulator are on-line computed as functions of the optimal - unbiased, minimum variance - estimates of the joint coordinates and velocities, and the corrected accelerations. The optimal state estimator based on a stochastic discrete-time linear dynamic model and previous observations is generated the discrete Kalman filter. Monitoring schemes for real-time detection and isolation of failures in robotic manipulators are outlined in the second part of the paper. The problem of fault detection is formulated as a problem in Hypothesis Testing by regarding the normal operation of the robot manipulator as the null hypothesis.

*Key-Words: - Robot control, Performance Monitoring, Kalman-filter, Fault diagnosis*

## 1 Introduction

The problem of robot control is mainly due to the nonlinear and coupled system dynamics. One of the most well known robot control schemes is the "computed torque control" method which involves the computation of the appropriate input generalized forces based on the robot dynamic model, using the measured values of the generalized coordinates and velocities and the computed values of the generalized accelerations. If the robotic manipulator model and the load are precisely known, the sensors and actuators are error-free, and the environment is noise-free the computed torque method assures that the trajectory error goes to zero. Gilbert and Ha have shown (Gilbert and Ha 1984, [1]) that the computed torque control method is robust to small modeling error. In practice, however, the available robot model is only an approximation of the equation of motion. Discrepancies between the model and the equations of motion may arise from several factors such as inaccuracies on inertias, masses and geometry, uncertainties on the friction terms, and the necessary model simplification. In addition, the robot carries variable or not precisely known load, the sensors and actuators have finite limits and precision, and the robots are often subject to random disturbances. This chapter addresses the problem to design the control inputs necessary to track a desired trajectory that has been planned for the robot manipulator, while simultaneously rejecting unknown disturbances.

Basically three control schemes are proposed

- The first one is a modification of the computed torque method where the input torques necessary to drive the robotic manipulator are on-line computed as a function of the optimal - unbiased, minimum variance - estimates of the joint coordinates and velocities, and the corrected accelerations. The optimal state estimator based on a stochastic discrete-time linear dynamic model and previous observations is generated by the discrete Kalman filter (Rudas and Bencsik 1990, [2]).
- The second group of control methods is improvement of the first one. The idea of the improvement of the control system is to use a better reference trajectory for linearization in each working cycle. Two ways are given to perform this relinearization process: the relinearization is performed around the estimated trajectory, and the extended Kalman filter (Rudas and Nagy 1992, [3]).
- The third novel control method addresses the problem to design a nonlinear feedback controller in case of uncertain dynamical parameters and other disturbances. In this control algorithm the input torques necessary to drive the robot manipulator are also on-line computed as a function of the unbiased, minimum variance estimates of the joint coordinates and velocities, and the corrected accelerations but the state estimator is given by the discrete Schmidt-Kalman filter. This filter takes into account the effect of uncertain parameters without estimating the parameters themselves.

A failure in a robot manipulator or in a sensor used to provide a feedback signal in a robot control system can cause a serious deterioration in the dynamic performance of the system. If the failure occurs gradually and is detected in its early stage the faulty part of the robot can be repaired or replaced before serious trouble develops.

Monitoring schemes for real-time detection and isolation of failures in robotic manipulators are outlined (Rudas 1991, [4]), (Rudas and Horváth 1993, [5]). The problem of fault detection is formulated as a problem in Hypothesis Testing by regarding the normal operation of the robot manipulator as the null hypothesis. The actual error signal from the robot manipulator is tested against the null hypothesis at a certain level of significance. The error signal, i.e. the innovation sequence, is defined as the difference between the actual robot manipulator output and the unbiased, minimum-variance estimate of the output based on the previous observations. The estimations are generated by the Kalman filter using a discrete-time linear model of the robot, which is obtained from the usual second-order differential vector equation after linearization and discretization.

There are a number of possible statistical tests to be performed on the innovations. One of this is the chi-squared test which can be implemented to robots but does not directly yield any information about in what joint the failure occurred. Such a fault isolation method is provided by the proposed test method. The problem of test to isolate system or sensor faults is formulated as a problem in analysis of variance by regarding the normal operation of the system as the null hypothesis.

## 2. The System Model

### 2.1.The Dynamic Model

The robot mechanism is modeled by an articulated chain of N rigid bodies which are serially connected by revolute joints. As we have seen the dynamic model can be written in matrix-vector form as,

(1)

The state of the robot, as modeled in (2.1.1), can be uniquely defined, at any particular instant of time, by the N joint coordinates and the corresponding N link velocities These 2N independent physical state-variables contain sufficient information to describe the position and velocity of the robot manipulator. By selecting the state x of the robot to be the (2N x 1) vector

(2)

the nonlinear state-space formulation of the dynamic model (2.1.1) is

(3)

or

(4)

A trajectory is defined as a continuous map of the time interval to the state space

(5)

with x(t0), x(tg) X, which are the initial and goal states, respectively.. Suppose that the robot executes the same sequence of motion and a reference deterministic trajectory xre(t) (this can be the prescribed trajectory) is generated with given xre(t0) satisfying equation (2.1.4). Define

(6)

as the deviation of the actual state from the reference trajectory. The nonlinear model can be replaced by its first order approximation

(7)

where is the deviation of the generalized forces from the nominal values,

are matrices of the linearized system given by

(8)

(9)

The resulting time-varying state-space model is obtained in the form

(10)

where is the N x N unit matrix, (t).

By introducing the matrices A(t) and B(t) the state-space equation is

(11)

### 2.2.The Complete Linear and Discrete Model

The dynamic model of the N-th actuator can be written of the following state space form (Vukobratovich and Potkonjak1982, [6]):

(12)

where , i is the vector of the rotor currents, and u is the vector of control voltages.

The robot manipulator model (2.1.1) and the actuator model (2.2.1) can be united into a new state space equation of the form:

.(13)

Let *t is the sampling interval, tk = t0 + kt, (k = 1,2,...,np, tp = t0 + npt*). The above equation can be discretized by numerical integration over interval [tk, tk +1]. The obtained linearized and discrete system is

.(14)

### 2.3 The Stochastic System Model

In an ideal situation the manipulator model is "exact" the dynamical parameters and the load are precisely known, the sensors and actuators are error-free, and the environment is noise-free. In industrial environment, however, the robot dynamics and the manipulator model are relatively accurate, the manipulator carries variable or not precisely known load, the sensors and actuators have finite limits and precision, and the robots are often subject to random disturbances, so that the industrial robots may themselves be random. Taking these effects into account the robot can be modeled by the stochastic N-vector differential equation.

(15)

where is a white Gaussian noise sequence. Its mean and covariance are

(16)

(17)

where is the Kronecker delta. We include the in the dynamics to account in an approximate way for errors due to neglected nonlinearities and other unmodeled disturbances. The initial condition is also assumed to be random. Its distribution is Gaussian with mean and

.(18)

The measurement equation is assumed of the form

(19)

and is a white Gaussian noise sequence with mean and covariance

(20)

(21)

## 3.Unified Control and Performance Monitoring System

### 3.1. The Control Algorithm

The idea of the control algorithm is to compute the joint torques in each time instant from the unbiased, minimum variance estimate of the joint coordinates and velocities ,

(22)

by using the corrected acceleration

(23)

where are the desired position, velocity and acceleration, is the position gain, an is the velocity gain. The architecture of the unified control and performance monitoring system can be seen in Fig.1.

Fig.1. The architecture of the unified scheme

### 3.2. Generation of Estimates

The optimal (unbiased, minimum variance) state estimator based on observations up to time k-1 is given by the discrete Kalman-filter equations (Jazwinski 1970, [7])

(24)

(25)

(26)

where the gainis calculated from the equations

(27)

(28)

(29)

(30)

Here is the estimation error covariance of the estimate . The innovation sequence is a zero mean Gaussian white noise process with known covariance .

### 3.3. Performance Monitoring

#### 3.3.1. Hypothesis Testing for Fault Detection

The problem of fault detection can be formulated as a problem in Hypothesis Testing by regarding the normal operation of the system as the null hypothesis. For Hypothesis Testing purpose, it is more convenient to consider the Standardized Innovation Sequence

(31)

where denotes the square root of the inverse of the matrix . If the robot works under normal conditions is a white Gaussian zero mean sequence with the identity matrix as its covariance.

Thus the statistics of are time-invariant. Consider the product

.(32)

As is known is a Chi-square random variable thus the Chi-square test can be used to determine whether the robot arm behavior is normal or not. Suppose that the average value over the last L observations is examined

.(33)

The hypothesis to be tested is that is if the robot behavior is normal we would expect remain below a threshold K.

If a fault occur (the hypothesis), the increases, and will be above the threshold. The size L of the average interval and the threshold K are design parameters. If K increases, the probability of false alarm (declaring when actually ) decreases, but the probability of miss alarm (declaring when actually ) increases. The choice of suitable parameters are to be carried out by simulation.

#### 3.3.2. Fault Isolation

It is possible to obtain information in what joint the fault occurred by considering the components of separately. Assumed that the state vectors partly observed, i.e. only the joint coordinates are measured. In this case we can consider

(34)

where is the associated variance. The detection rule is:

if then no fault,

if then fault.

Denote by the realization of the trajectory (i = 1,2,...); and suppose that in the realization, in the join and at time a fault has been detected.

As (i = k-L+1,...,k), are off-line computed data, only any of the could have been changed. We have

(35)

As a consequence of these, at time any of the

(36)

could have increased, which means that one of the following three types of faults could have been occurred:

*Dynamic system fault– changes in expression

increasing the value of(37).

*Sensor fault - biased sensor noise.

*Both system and sensor noise.

To identify the type of fault the analysis of variance will be used. This technique employs tests, based on variance ratios to determine whether or not significant differences exist among the means of groups of observations, obtained at time by the realization of the cycle, where each group follows a normal distribution.

#### 3.3.3. Test to identify system fault

Define the random variables

(38)

l = 1,2,...,m

i = k-L+1,...,k.

As is known

(l = 1,2,...,m) are zero mean, independent random variables, following the same normal distribution (Korn and Korn 1975, [8]).

Consider the standardized random variables

(39)

where is the diagonal element of the matrix . The mean and the variance of are thus time-invariant.

The problem of test to identify system fault is formulated as a problem in analysis of variance by regarding the normal operation of the system as the null hypothesis. If the system behavior is normal the statistics of does not change. Denote the expected value and the variance of (l = 1,2,...,m). The normal system operation will mean the equality of these variances and expected values.

First consider the null hypothesis

In testing the Bartlett test (Sarkadi and Vincze 1974, [9]) is applied.

Denote by the empirical variances and introduce the notation

.(40)

The following test statistic is applied:

(41)

where

(42)

It is known that the statistic in Eq.(41) has an approximate distribution with L-1 freedom when used as a test statistic for. The rejection region at level will be

If is accepted we go on with the examination of the null hypothesis

against the alternative

:at least two are not equal to each other.

To test the hypothesis the analysis of variance will be used (Sarkadi and Vincze 1974, [9]). For making a decision concerning the, at level the following F-statistic is used:

(43)

where

(44)

with

(45)

and

(46)

If holds, then will be accepted. Otherwise we have at level the rejection region is of the form

#### 3.3.4. Test to identify sensor fault

The test to identify sensor fault is carried out analogously to the test formulated to identify system fault with the exception that will be defined as follows

(47)

l = 1,2,...,m

i = k-L+1,...,k.

where is the diagonal element of the matrix .

The are white, independent random variables with standard normal distributions.

## 7. Conclusions

The paper presents an approach for robot control which is based on the discrete Kalman Filter Algorithm. The second part of the paper discusses a monitoring scheme for real-time detection and isolation of failures. The actual error signal from the robot manipulator is tested against the null hypothesis at a certain level of significance. The error signal, is defined as the difference between the actual robot manipulator output and the unbiased, minimum-variance estimate of the output based on the previous observations. The estimations are generated by the Kalman filter using a discrete-time linear model of the robot.

## 8. Acknowledgment

The author gratefully acknowledges the support by the Hungarian National Research Fund OTKA in the projects T34651, T034212and T 037304.

## 9.References

[1]Gilbert, E.G. and Ha, I.J. [1984]: An approach to nonlinear feedback control with application to robotics. IEEE Trans. Systems, Man, Cybern. SMC-14: 879-884.

[2]Rudas, I., Bencsik, A. [1990] Application of Linear Filtering in Industrial Robot Control *Proceeding of the IEEE International Workshop on Intelligent Motion Control. *Istambul, Aug. pp. 837-840.

[3]RudasI. J., N-Nagy, F. L. [1992] Advanced Industrial Robot Control Using Extended Kalman Filter. IFAC Symposia Series, No. 4. Vol. Robot Control (SYROCO ’91), pp. 135-140.

[4]Rudas, I. J. [1991] Fault Detection in Robots Using Stochastic Filtering. *Proceedings of the 1991. Int. Conf. on Industrial Electronics, Control and Instrumentation, (IECON’91) *Kobe, Japan, pp. 1147-1152.

[5]Rudas, I. J., Horváth, L. [1993] Fault Detection in Industrial Robots Using a Learning Control Based Approach. *Proceedings of the 32nd SICE Annual Conference. *Kanazawa, Japan, 1993.

[6]Vukobratovic, D.,Kircanski, N*.[1985a]: Real-Time Dynamics of Manipulation Robots*. Springer-Verlag. Berlin, Heidelberg, New York.

[7]Jazwinski, A.H. [1970]: *Stochastic Processes and Filtering Theory*. Academic Press, New York and London.

[8]Korn, G.A., .Korn, T.M. [1975]: *Mathematical Handbook for Scientist and Engineers*. Mûszaki Könyvkiadó, Budapest.

[9]Sarkadi, K, Vincze, I. [1974]: *Mathematical Methods of Statistical Quality Control*. Publishing House of the HungarianAcademy of Sciences, Budapest.

[10]Spong, M.W and Vidyasagar, M. [1989]: Robot Dynamics and Control, John Wiley & Sons.

[11]Vukobratovic, M., Stokic, D*.[1982]: Control of Manipulation Robots*. Springer-Verlag. Berlin, Heidelberg, New York.

[12]Schmidt, S.F. [1966.]: Application of State-Space Methods to Navigation Problems. Advanced Control Systems, 13, pp. 293-340.