International Journal of Advanced Robotic Systems Target Tracking for Visual Servoing Systems Based on an Adaptive Kalman Filter Regular Paper

which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited. Abstract Visual servoing has been around for decades, but time delay is still one of the most troublesome problems to achieve target tracking. To circumvent the problem, in this paper, the Kalman filter is employed to estimate the future position of the object. In order to introduce the Kalman filter, accurate time delays, which include the processing lag and the motion lag, need to be obtained. Thus, the delays of the visual control servoing systems are discussed and a generic timing model for the system is provided. Then, we present a current statistical model for a moving target. A fuzzy adaptive Kalman filter, which is evolved from the Kalman filter, is introduced based on the current statistical model. Finally, a two DOF visual controller based variable structure control law for micro‐manipulation is presented. The results show that the proposed adaptive Kalman filter can improve the ability to track moving targets. 1. Introduction In the past ten years, microscopic visual servoing systems have been used widely in micromanipulator control for catching a target and mounting a part. When the object to be manipulated is controlled, which means that the object is static or the trajectory of the object is known, visual servoing control is easily achieved. Papanikolopoulos [1] presented algorithms to allow an eye‐in‐hand manipulator to track unknown‐shaped 3‐D objects moving in two‐dimensional (2‐D) space. The object's trajectory was either a line or an arc. Hashimoto [2] proposed a visual feedback control strategy for an eye‐in‐ hand manipulator to follow a circular trajectory. However, when the object can't be controlled, i.e., the object's trajectory is uncertain, the visual servoing system often performs poorly due to the low sampling rate of the cameras and the time delays, which are created by image processing and motion control. A lot of similar problems have been discussed in the literature [3‐5]. From the above systems, we notice that the motion of the object was smooth and limited to a known class of trajectories.


Introduction
In the past ten years, microscopic visual servoing systems have been used widely in micromanipulator control for catching a target and mounting a part. When the object to be manipulated is controlled, which means that the object is static or the trajectory of the object is known, visual servoing control is easily achieved. Papanikolopoulos [1] presented algorithms to allow an eye-in-hand manipulator to track unknown-shaped 3-D objects moving in two-dimensional (2-D) space. The object's trajectory was either a line or an arc. Hashimoto [2] proposed a visual feedback control strategy for an eye-inhand manipulator to follow a circular trajectory. However, when the object can't be controlled, i.e., the object's trajectory is uncertain, the visual servoing system often performs poorly due to the low sampling rate of the cameras and the time delays, which are created by image processing and motion control. A lot of similar problems have been discussed in the literature [3][4][5]. From the above systems, we notice that the motion of the object was smooth and limited to a known class of trajectories.
In this paper, we seek to extend this to let the robot follow a completely unknown trajectory.
There are two difficulties to be tackled in the task of tracking a moving object with a microscopic visual servoing system [6,7]. The main difficulty is the delays, which include two parts: the processing lag and the motion lag. The processing lag is introduced by the time needed to acquire and process the image, while the motion lag is produced because the slow rate of update for visual feedback results in larger-than-desired motions between updates. The second difficulty is that the maximum possible rate for visual sensing and processing is much slower than the minimum required rate for mechanical control. A common approach to overcome these problems, especially the time delays, is to introduce predictive algorithms, such as the Kalman filter [7].
The Kalman filter is well known for its use in optimal estimation and is especially suitable for a system with disturbances and measurement errors [8]. The traditional Kalman filter formulation assumes complete a priori knowledge of the process noise covariance matrix Q(k) and the measurement noise covariance matrix R(k). However, in most practical applications, these matrices are initially estimated or, in fact, are unknown. The problem here is that the optimality of the estimation algorithm in the Kalman lter setting is closely connected to the quality of these a priori noise matrices. It has been shown how poor estimates of the input noise matrices may seriously degrade Kalman lter performance and even provoke divergence of the lter [9]. Calculation of matrix R(k) for a particular measurement system is a straight-forward process. But obtaining the matrix Q(k) involves extensive experimentation and complex mathematical analysis. It is also not guaranteed that the matrices Q(k) and R(k) remain constant with time in highly non-stationary noise conditions. Thus, it is imperative to continuously tune the Kalman filter accounting for the changing noise conditions in order to get good filter performance.
Many adaptation schemes for tuning the Kalman filter were previously developed, following both conventional adaptation [10] and Fuzzy Logic adaptation [11]. Moghaddamjoo and Kirlin propose a method to adapt the Kalman filter to changes in the input forcing functions and the noise statistics [12]. Mohamed and Schwarz propose an adaptive Kalman filter which shows a major improvement over the conventional one through the adaptation of R(k)/Q(k) [13]. The performance of the adaptive Kalman filter, for most of the navigation parameters used, is improved by almost 50% or more when compared to that of the conventional filter. The main advantage of the adaptive technique is its weaker reliance on a priori statistical information. The combination of fuzzy logic and the Kalman lter is not new. Some researchers [14,15] present probabilistic Kalman lters for mobile robots, where fuzzy rules are used to adapt R(k)/Q(k). Escamilla-Ambrosio and Mort presented a fuzzy inference system to adapt the measurement noise covariance matrix R(k) or the process noise covariance matrix Q(k) of a Kalman filter [16]. It was observed that better performance was obtained with the fuzzy adaptive Kalman filter than that obtained with both a Kalman filter without adaptation and a traditional adaptive Kalman filter.
In this paper, we introduce a novel adaptive Kalman filter. Based on the current statistical model, we propose a developed adaptive Kalman algorithm, which estimates the process noise covariance matrix Q(k) by a new formula (10). Then, an improved fuzzy adaptive Kalman filter is applied to estimate the covariance matrix R(k) and eliminate the influence of the history data. The experimental results show that the improved fuzzy Kalman filter can reduce prediction error and sense the variation of motion faster. The results are compared with those obtained using a conventional Kalman filter and a traditional fuzzy Kalman filter [16]. The improved fuzzy adaptive Kalman filter shows better results than its traditional counterparts.
The main task of the algorithm is to predict the position of the object in the next period of the image. For achieving this goal, we should first know accurately the delay time of the visual servoing system. So, a generic timing model for the visual servoing system is proposed to get the accurate delay time [6]. In order to describe the target motion, a current statistical model is presented [17][18][19]. In the current statistical model, the target's acceleration is characterized by a modified Rayleigh probability density function and time-correlated by its mean value. Thus, the current statistical model can detect the target manoeuvre accurately and quickly.
To meet the demands of a high-precision tracking task, the visual servoing method is employed to guide the end effectors towards the moving object. Since the system's precise calibration is complicated and difficult, especially for micro-manipulation, we use the uncalibrated method to estimate the image Jacobian matrix online [20]. Then, a variable structure controller [21,22] is applied to our micro manipulation system to track a moving object. The variable structure control is robust to parameter uncertainty and external disturbance because of the sliding motion on a predefined hyper plane. The experiments of visual servoing with the improved fuzzy Kalman filter in microscopes confirm that the proposed method is effective and feasible.
This paper is organized as follows: In Section 2 we introduce our generic timing model. Section 3 describes a current statistical model to express the motion of the object. In Section 4 an improved fuzzy adaptive Kalman filter is introduced. In section 5 visual servoing based on the fuzzy adaptive Kalman filter by using a variable structure controller is detailed. Experimental results on the performance of the proposed method are illustrated in Section 6 followed by the conclusion in Section 7.

Generic Timing Model
The time delay in visual servoing systems includes two parts [6]: processing lag and motion lag. The processing lag is introduced by the time required for visual sensing and processing between when the object state in reality is sensed and when the visual understanding of that object state (e.g., image tracking result) is available. The motion lag is produced by the slow rate of update for visual feedback results in larger-than-desired motions between updates when the mechanical system has completed the desired motion. It is worth noting that the maximum possible rate for complex visual sensing and processing is much slower than the minimum required rate for mechanical control. The first step of visual servoing is visual sensing and processing. There are four main configurations for visual sensing and processing, which are serial, on-the-fly, parallel and pipeline processing. Fig. 1 shows the four cases, where tac presents the visual sensing time, tp presents the visual processing time.
According to Fig. 1, on-the-fly processing has the shortest delay time, but tac is usually smaller than tp in the actual system. Compared to the other three architectures, parallel processing is efficient and doesn't increase the complexity of the system. Hence, we employ parallel processing to construct the timing model. Fig. 2 depicts the proposed timing model. There are two input buffers (commonly called "double buffering"). In this case, a second image buffer is being filled while the image data in the first buffer is being processed. Double buffering increases the throughput. In Fig. 2, tac presents the image acquisition time, tp presents the image processing time, tc1 presents the time that the visual controller outputs the control signal, tc2 presents the servoing time of the robot joint and tm presents the time that the manipulator takes to move from the current position to the desire position.
According to Figure 2, the precise delay time T is: (1)

Current Statistical Model
Based on the timing model, the current statistical model is introduced to describe the motion of the target. The current statistical model is a Singer model modified to have non-zero mean, time-correlated acceleration [19]. It is assumed that the acceleration of the target ) (t a is satisfied with the equation as following: is the mean of acceleration, assumed to be constant in every sampling period. The current statistical model based on the visual servo system is: , x and y are the predicted coordinates of the moving object in the next image which will need to be processed, the predictive velocity of the object in the image coordinate system is presented by x  and y  ; x  and y  represent the predicted acceleration of the object in the next image, while is the coordinate of the object that is measured in the current image; α is the acceleration correlation coefficient; V(k) is the zero-mean gauss white noise, whose variance is R(k). The process noise W(k) is a discrete time sequence of white noise and Process noise covariance matrix Q(k) is given by: Where: (   T   T  T   T  T   T  T   T  T  T   T  T   e  q   e  e  q   T  e  e  q   Te  e  q   T  T  Te  e  e  q   Te  T  T  T Variance σ is the modified Rayleigh distribution variance of the manoeuvring acceleration: Where max a is the positive upper limit of the acceleration, while max  a is the negative lower limit of the acceleration.

Improved Adaptive Kalman Filter
For any model of the form (3), the prediction can be calculated using a Kalman filter [23]. In more detail, we have is a prediction of the hidden model state, is the predicted covariance matrix of the state and ) (k K is called the Kalman filter gain. Looking at the above formulae in more detail, we can see how process noise are large, the system has a quick response with lower tracking accuracy. To cover this defect, we introduce a developed adaptive Kalman algorithm [24]. Let:

Estimate the Covariance matrices
In this section, a fuzzy adaptive Kalman filter is applied to estimate the covariance matrices . Fuzzy control is one of the useful control paradigms for uncertain and ill-dened nonlinear systems. Control actions of a fuzzy controller are described by some linguistic rules. We adjust , which is defined as: The innovation sequence represents the information of the new observation and is considered the most relevant source of information for filter adaptation. In theory, the innovation sequence is a zero mean gauss white noise sequence: The theoretical covariance matrix can be derived from the Kalman filter: However, in practice, the innovation sequence is affected by model uncertainty and noise statistical uncertainty. We use [25]'s method to obtain the equalizing value and covariance matrix of the innovation sequence: N is chosen empirically to give some statistical smoothing. Then, we get the error formula between the theoretical covariance matrix and the practical covariance matrix of the innovation sequence: Notice that E and P   , whose values should be zero in an optimal situation, reflect the state of the current Kalman filter.
The range of β is [0. 8 1.2], while the range of E is [-0.5 0.5] and the range of P   is selected as [- 10 10].   does not really provide completely new information because some of the information is obtained by predictions from previous filter states, which contain all the predicted information from . If the object's motion is changed, the old data can't reflect the current motion of the object. When the motion changes abruptly, the Kalman filter can't follow the variation in time, due to the influence of the history data. The variation of the motion can be monitored by the innovation sequence. In order to let the Kalman filter sense the variation of the motion faster, we 3) according to the variation of the innovation sequence by the fuzzy controller. We rewrite (9.3) as follow: We calculate the error between the current equalizing value and the last equalizing value of the innovation sequence as follow:

According to the adjustment rules, if the value of
is greater than the last equalizing value of the innovation sequence, will be greater than the original value of . Hence, the influence of the innovation sequence in (9.2) will increase.

Visual Servoing Based on the Fuzzy Adaptive Kalman Filter
Visual servoing methods can be categorized into two basic groups: position-based visual servoing and imagebased visual servoing (IBVS) [26]. In IBVS, a set of features is selected and fed back to the controller through an image Jacobian matrix, which represents a local, linear and differential relationship between the velocities of the image features and the joint velocities. IBVS is less sensitive to image disturbances and calibration errors compared to a position-based approach and it is inconvenient to describe tasks using image features. The position-based visual servoing needs precise calibration of the intrinsic parameters of the camera. However, system calibration is the complicated and difficult problem, especially for micro-manipulation based on microscope vision. So, our approach to visual servoing is the uncalibrated image-based one. For IBVS, the first task is to identify multi micro parts. The improved support vector machine (SVM) multi objects' identification method is used to fulfil this task [20]. The image Jacobian matrix plays a major role in IBVS. In our previous work, we used Broyden's method to estimate the image Jacobian matrix [20]. The micro manipulation tasks will be divided into horizontal (XY plane) movement and vertical (Z axis) movement. The manipulator moves in the XY plane first positioning small parts, then does so in the Z-axis, positioning small parts at the centre. Therefore, we apply two image Jacobian matrices, including a horizontal view of the image Jacobian matrix and vertical view of the image Jacobian matrix, which can complete the positioning and tracking of three-dimensional objects.
In this section, the variable structure controller is applied to our micro manipulation system to track a moving object. The variable structure control (VSC) has been widely discussed and used in various applications. The control function of VSC is designed to reach the sliding surface that is predetermined to achieve the desired performance. It is gaining importance as a universal design methodology for the robust control of linear and nonlinear systems. It offers several redeeming features from a control theory point of view [27,28], namely, an inherent order reduction, direct incorporation of robustness against system uncertainties and disturbances and an implicit stability proof. In recent years, due to its robustness, the application of VSC on dynamic systems subject to external disturbances has been extensively studied. Many desirable properties of VSC systems are well documented [21,22]. According to the results of previous studies on sliding surface design approaches, the performances of the controlled systems were satisfactory. Li and Xie propose a visual servoing tracking controller based on the sliding mode control theory, which exploits the epipolar geometry between a current image and a target one without requiring any a priori knowledge of the scene or precise camera calibration due to the intrinsic robustness of the control law against matched uncertainties and disturbances [29]. Simulation results showed that the control scheme can achieve good tracking performance and has strong robustness against parameter variations and external disturbances. Hence, we will focus here on a variable structure control with sliding mode, which proved its potential in visual servoing system [29].
Since the micro manipulation task is divided into horizontal (XY plane) movement and vertical (Z axis) movement, we design two DOF visual controller based VSC laws for micro manipulation.
The state equation for a visual servoing system can be written as: is the image Jacobian matrix, T is the sampling period of the vision system and is the manipulator endeffector velocity. The control objective of the visual servoing is to control the end-effector to achieve the desired image plane coordinate, which could be constant or changing with time.
Based on the state equation, we choose the switching plane of VSC as: is the desired image plane coordinate. We already know that when the object is moving, the desired image plane coordinate observed at the current time will be not the same as the desired image plane coordinate which the manipulator should move to at the current time, due to the time delay of the visual servoing system. Hence, we should estimate the actual desired image plane coordinate. (20) can be rewritten as: is the estimated value of the actual desired image plane coordinate by using the fuzzy adaptive Kalman filter which is described in Section 4 and: The ) (k u is characterized by the control structure defined by: (23) According to VSC, when the sliding mode occurs, should be zero, so we have: We choose: Where: and: Substituting (25) into (24), we have: In Eq. 29, the coefficient of , affects the dynamic property of the visual servoing system. If the value of W C 1  is large, the tracking error can be eliminated as quickly as possible. When the end-effector is far from the moving target at the beginning of the visual servoing, this will cause excessive velocity and displacement of the end-effector. If the value of W C 1  is small, the tracking error can be reduced smoothly. However, the final tracking error is large. Hence, at the beginning of the visual servoing, the value of W C 1  should be small. When the end-effector moves close to the target, we should increase the value of W C 1  to make the tracking error as small as possible.

Simulation
Based on the current statistical model identified in Section 3, we firstly simulate the system to evaluate the prediction performance of the Kalman filter without adaptation, the traditional fuzzy adaptive Kalman filter proposed in [16] and our improved fuzzy adaptive Kalman filter.  Three kinds of prediction task are simulated. The first task is tracking a target which is moving uniformly and in a straight line. The starting position of the target in the image coordinate system is x=0, y=0 and the velocities in the x-direction and the y-direction are both 10 mm/s. The second task is tracking a motion where the acceleration changes sharply during the movement. At the beginning, x=0, y=0. The velocities in the x-direction and the ydirection are both 20 mm/s, while the accelerations are both 1mm/s 2 . After 14 seconds, the accelerations in the xdirection and the y-direction change to 30mm/s 2 . The zigzag motion tracking is the third task. The beginning position of the object is x=0, y=0. The velocity in the xdirection is 1mm/s. The y-direction velocity is 10mm/s, which changes to -10mm/s after 9 seconds. The prediction error is used as a criterion to compare the different computation methods and is defined as: is the real measurement and The results of the simulation are shown in Fig. 5. For comparison, we show the simulation results based on the Kalman filter without adaptation, the traditional fuzzy adaptive Kalman filter proposed in [16] and our improved adaptive Kalman filter respectively.
In Fig. 5(a), the results show that the performance of the improved fuzzy adaptive Kalman filter is better than the Kalman filter without adaptation and the traditional fuzzy adaptive Kalman filter [16]. We can get the best response and the smallest overshoot with the improved adaptive Kalman filter if the object remains in a state of uniform motion in a straight line. This effect can be observed more clearly in the Fig. 5(b) and Fig. 5(c) when the task is tracking a motion where the acceleration changed sharply during the movement and a zigzag motion. In the tracking task, if the response is poor and the overshoot is big, the camera may lose a target whose velocity or acceleration changes greatly when the tracking window is small. However, the processing time of enough large images would be too long. The improved adaptive Kalman filter can track an object with a small tracking window. It can improve system efficiency without losing the tracked object.

Visual Servoing Experiment
Object tracking is an important component of a micro visual servoing based micro assembly system [20,30]. The system is controlled by two levels of computer control systems and consists of three parts: a micromanipulator unit, a stereo microscopic vision unit and a micro grippers unit. A triple-manipulator cooperation strategy was developed in the system to fulfil 3D micro assembly tasks. Compared with the common stereo vision configuration using two parallel views, a microscopic vision unit was developed with two perpendicular views to reduce the structural complexity of the mechanism, which includes the horizontal microscope vision and the vertical microscope vision. In the micro gripper unit, there are two kinds of micro grippers: a vacuum micro gripper and a piezoelectric (PZT) bimorph micro gripper. Fig. 6 shows the visual servoing micro manipulation system. Fig. 7 shows the original image of the operation targets and the manipulator in the microscopic environment for a vertical microscope.  In this experiment, we first attempt to predict the centre of the micro gripper in different kinds of tracking task. Fig. 8 is the image after edge extraction of the operation targets and the manipulator in the microscopic environment for a vertical microscope. (b) Tracking a manipulator that is describing a circular motion with uniform velocity Figure 9. The prediction error between the centre of the manipulator and the centre of the window The first task is predicting a micro gripper that is moving uniformly and in a straight line with a velocity of 1mm/s. The second task is predicting a micro gripper that is describing a circular motion with uniform velocity, whose radius is 20mm and angular velocity is 0.15rad/s. We use a window of 150×150 pixels to track the micro gripper, see Fig. 7. For comparison, we show the results based on the Kalman filter without adaptation, the traditional fuzzy adaptive Kalman filter proposed in [16] and our improved adaptive Kalman filter respectively. The prediction error between the centre of the manipulator and the centre of the window is shown in Fig. 9. Fig. 9(a) verifies that our improved fuzzy adaptive Kalman filter has a faster response and a smaller overshoot if the object remains in a state of uniform motion in a straight line. The final error of the improved fuzzy adaptive Kalman filter is zero. The Kalman without adaptation and the traditional fuzzy adaptive Kalman filter have the same final prediction error, which is about 1 pixel. When the micro gripper is describing a circular motion with uniform velocity, the prediction error of the improved fuzzy adaptive Kalman filter reaches a small value quickly in the first 2 seconds, however, the prediction error of the Kalman filter without adaptation and the traditional fuzzy adaptive Kalman filter remain at a large value. The final prediction error using the Kalman filter without adaptation is about 8 pixels and around 4 pixels using the traditional fuzzy adaptive Kalman filter, while the final prediction error remains at 2 pixels as time goes on using the proposed method as shown in Fig. 9(b).
The results show that the proposed method has better performance.
(a) X-direction error (b) Y-direction error In order to verify the performance of the visual servo system, two kinds of tracking tasks are studied. In the first task, we use right PZT bimorph micro gripper to track the left PZT bimorph micro gripper which is moving uniformly and in a straight line. In order to avoid collision of the two micro grippers, we set the image plane coordinate, which is the sum of a constant value and the image plane coordinate of the left PZT bimorph micro gripper, as the desired image plane coordinate at which the right PZT bimorph micro gripper should arrive.
The constant value is 300 pixels. The beginning position of left micro gripper is imx=100, imy=100. The beginning position of the desired motion is imx=100+300, imy=100.
The velocity in the x-direction is 0mm/s. The y-direction velocity is 1mm/s. The tracking errors between the two PZT bimorph micro grippers are shown in Fig. 10. It is worth noting that the constant value has been subtracted in order to show the tracking errors more suitably. From Fig. 10, we can see the tracking error of y-direction error is about 5 pixels when we don't introduce the Kalman filter. The appearance of the error is caused by the time delay of the visual servoing system as we discussed in the above sections. The improved adaptive Kalman filter can eliminate the error when the end-effector is tracking an object that moves uniformly in a straight line.
(a) X-direction error (b) Y-direction error Figure 11. The tracking error for tracking the left microgripper that is describing a circular motion with uniform velocity The second task is tracking the left PZT bimorph micro gripper that is describing a circular motion at with uniform velocity, whose radius is 20mm and angular velocity is 0.15rad/s, by using the right PZT bimorph micro gripper. The motion equation of the left PZT bimorph micro gripper is: The starting position of left PZT bimorph micro gripper is imx=200, imy=150. The centre of the circle is (100,150). Hence, the beginning position of the desired motion is imx=200+300, imy=150. The desired centre of the circle is (400,150). The starting position of the right PZT bimorph micro gripper is imx=510, imy=160. The tracking errors between the two PZT bimorph micro grippers of the second task are shown in Fig. 11. When the left micro gripper makes a circular motion at a uniform velocity, the final tracking error in both directions is equal or less than 4 pixels when using the proposed method and the traditional fuzzy adaptive Kalman filter [16], while the tracking error fluctuates between -10 pixels and 10 pixels. However, it can be seen that the tracking error when using the traditional fuzzy adaptive Kalman filter is larger than when using the improved fuzzy adaptive Kalman filter at the beginning of the tracking task. The experimental results support the superiority of the proposed method.

Conclusion
In this paper, we propose an improved adaptive Kalman filter for a visual servoing system to track a moving target. This method can not only track an object which moves smoothly, but also whose velocity or acceleration changes greatly. In order to introduce the improved Kalman algorithm, we establish the generic timing model of the visual servoing system and build a current statistical model which can describe the trajectory of a moving target. Based on the current statistical model, we propose a developed adaptive Kalman algorithm, which estimates the process noise covariance matrix Q(k) by an improved formula. Then, an improved fuzzy adaptive Kalman filter is applied to estimate the covariance matrices R(k) and eliminate the influence of the history data. The simulation and experimental results show that the improved fuzzy Kalman filter can reduce the prediction error and sense the variation of the motion faster. Finally, a variable structure controller is used to complete the visual servoing task of target tracking, the results show that tracking performance is improved by introducing the improved adaptive Kalman filter. The choice of parameters for the variable structure controller is important to the dynamic property of the visual servoing system. Our future work is to investigative a method which can estimate the parameters' self-adaptability.