Keywords

1 Introduction

Nowadays vision systems are fest-growing technology which is getting more and more usefull in everyday of life (mobile phones—camera with face recognition algorithm, smart TV—geasture control as well as industry e.g. quality control [1], products classification [2], monitoring systems [3], items sorting in cooperation with robots [4, 5]. Many of them need a special algorithms which will be robust for disturbances. These algorithms are divided into two groups. First refers to features detection [6, 7]. Second group is focused on marker detection. Also three groups of markers are known. The passive markers [8], active [9, 10] and coded [11]. Passive markers are made of color paper or special material which allows to reflect the light, it is usually used in 3D measurement vision system [12]. The reflective marker is illuminated by infrared light, which should be robust for Sun rays. In many cases, when some parts of markers (either passive or active) are invisible, the recognition is difficult, or in worst scenerio impossible. Therefore most popular markers have circle-like shape [13], it is claimed that this is an optimal configuration [14]. However, when the user accidentally covers small parts of it, the coupled vision system does not guarantee that markers will be well captured. That can yield problems with detection and, what is more, tracking of markers central points. Thus it is important to search for a new implementable algorithm to recognize and filter markers features. The presented study is a continuation of a research given in [15]. The authors proved that in the problem of marker’s position tracking, the Kalman Filter turned out to be more accurate than complementary filter. Therefore here, only the previously undertaken methodology will be considered and applied in control of a electro-hydraulic manipulator.

2 The Testbed

The tests were performed on a electro-hydraulic manipulator equipped in proportional valves, an industrial controller and the vision system.

Fig. 1
figure 1

3iCube CMOS camera [16]

Full size image

Images were captured by a fast camera 3iCube IC1500CU (Fig. 1) [16] with special software called Adaptive Vision Studio, which uses SEE multicore instructions [17]. This camera was connected to the PC computer by USB 3.0. The focal length of the camera lens is 3.5 mm. The max resolution of camera is equal to \(2592 \times 1944\) yielding 15 fps, but in current research the camera’s resolution was reduced to \(640\times 480\) px and image capture was performed with frequency up to 50 Hz. Calculations were performed on the personal computer. Next, the data with coordinates were sent to PLC (Programmable Logic Controller—B&R company) via TCP/IP protocol. The position of hydraulic cylinder was controlled by proportional valves connected with dedicated control card. Input signal in the card was in a range of \(-\)10..\(+\)10 VDC. The positions of angles of manipulator’s arms were measured by incremental encoders with resolution of 3600 impulses per revolution. The block diagram of the stand is shown in Fig. 2.

Fig. 2
figure 2

Block diagram of research stand

Full size image

The main algorithms of image processing and estimation of a central position of the markers is computed in two phases. The first phase is a basic processing of image:

  • image capture from the camera (RGB image),

  • Gaussian smoothing,

  • HSV filtration,

  • image thresholding to region,

  • region erode,

  • region dilate,

  • split region into blobs.

The second phase refers to marker’s features extraction. In this phase, a few steps can be highlighted. Firstly it is necessary to calculate the size of region:

$$\begin{aligned} S = \sum \limits _{i=1}^n \sum \limits _{j=1}^m p(i,j) \end{aligned}$$
(1)

where: p—is a position of the point on an image, \(n \times m\)—is a size of region, \(p(i,j)=1\)—when object exist, \(p(i,j) = 0\)—when object does not exist, Next, the center of mass of the region (\(x_m,y_m\)) must be found. It can be simply calculated with the following:

$$\begin{aligned} x_m = \frac{\sum \limits _{i=1}^n \sum \limits _{j=1}^m k}{S}, k={\left\{ \begin{array}{ll} \text {i - when object} \\ \text {0 - in other case} \end{array}\right. } \end{aligned}$$
(2)
$$\begin{aligned} y_m = \frac{\sum \limits _{i=1}^n \sum \limits _{j=1}^m k}{S}, k={\left\{ \begin{array}{ll} \text {j - when object} \\ \text {0 - in other case} \end{array}\right. } \end{aligned}$$
(3)

In the literature several fitting algorithms can be found. All of them find best circle and based on that, the central points of the markers are established. The most popular are proposed by Kasa [18], Pratt [19] and Taubin [20]. The main task of those algorithms is to minimize the cost of function:

$$\begin{aligned} \mathcal {F}(a,b,R) = \sum d_{i}^{2} \end{aligned}$$
(4)
$$\begin{aligned} d_i = r_i-R,~~r_i=\sqrt{(x_i-a)^2+(y_i-b)^2}, \end{aligned}$$
(5)

where: a, b—center of circle, R—radius of circle.

The performance of these algorithms was verified by Al-Sharadhad in [21]., but Chernov proved, that algorithms proposed by the Pratt and Taubin are more stable and accurate than the Kasa [22]. Therefore in this article the Authors will focus only on Pratt’s algorithm.

3 Implementation of Kalman Filter

The Kalman Filter is well-known recursive algorithm, which was originally used for linear problems [23]. During the last fifty years, the KF evaluated to deal with nonlinear models, however here the classic type of the KF is used. The overall framework of the KF assumes two-stages process. In the first one, namely prediction, the model data is exploited. During that stage some forecasts on state vector behavior are made. In next stage, the corrections are being imposed. The KF operate on process model which can be written in a state-space representation:

$$\begin{aligned} \begin{array}{c} \underline{x}_k = A \cdot \underline{x}_{k-1}+B \cdot \underline{u}_k \\ \underline{y}_k = C \cdot \underline{x}_k \\ \end{array} \end{aligned}$$
(6)

where \(\underline{x}_k\) denotes state vector, A, B and C refer to process, input and output matrices, respectively. In the fusion process proposed by the authors, the state vector contains positions (x,y) and velocities (\(v_x\),\(v_y\)) of a central of the marker.

$$\begin{aligned} \underline{x}_k=\left[ \begin{array}{c} x \\ v_x \\ y \\ v_y \\ \end{array} \right] , \end{aligned}$$
(7)

Matrices A and B are time-variant and thus they were extended with subscripts. The main process is incorporated in \(A_k\) and \(B_k\). It simply reflects the dynamics of the central point and it is given by:

$$\begin{aligned} \mathbf {A}_k= \left[ \begin{array}{c c} \mathbf {A}_k^z &{} 0_{2 \times 2} \\ 0_{2 \times 2} &{} \mathbf {A}_k^z \\ \end{array} \right] , \mathbf {B}_k=\left[ \begin{array}{c c} \mathbf {B}_k^z &{} 0_{2 \times 1} \\ 0_{2 \times 1} &{} \mathbf {B}_k^z \\ \end{array} \right] , \end{aligned}$$
(8)

where: \(\mathbf {A}_k^z = \left[ \begin{array}{c c} 1 &{} dt \\ - \frac{1}{dt} &{} 0\\ \end{array} \right] ,\) \(\mathbf {B}_k^z = \left[ \begin{array}{c } 0 \\ \frac{1}{dt}\\ \end{array} \right] ,\)

The output matrix \(C_k\) directly transfers the state vector to the output vector:

$$\begin{aligned} C=I _{ 4 \times 4}, \end{aligned}$$
(9)

The Kalman Filter equations are divided into two stages: The first is state prediction, which yields the a priori state vector \(\underline{\hat{x}}_k^-\), next it is necessary to compute a priori covariance matrix \(P_k^-\). One will notice, that by applying the first estimation of central point positions in \(\underline{u}_k\) the a priori estimation of velocities will be calculated. The KF is prepared in that way, so it can estimate the velocity of point coordinates (in X and Y axis) based on previous (k-1) a posteriori positions and the input. Also the positions are upgraded by integrating the a posteriori velocities.

In the second stage of Kalman Filter, namely correction, three main tasks are being executed. The first one is the Kalman gain \(K_k\) calculation, then the prior state vector \(\underline{\hat{x}}_k\) is corrected with measurement innovation. Finally the covariance matrix \(P_k\) is updated, obtaining the a posteriori covariance \(P_k\).

The Kalman Filter is tunned via the \(Q_k\) and \(R_k\) matrices. The \(Q_k\) refers to process noise, while the \(R_k\) stand for measurement noise.

4 The Experiment

The main aim of the research was to verify a new method of markers position estimation in case of simulated disturbances like covering or shading. During the experiment the saved video was used with dynamic disturbance like covering some parts of marker.

Fig. 3
figure 3

Control of electro-hydraulic manipulator with vision system. a Image after processing. b Electro-hydraulic manipulator

Full size image

Similar situation can occur when a person holds the marker in a hand and makes some movements. The incomplete information of marker’s position can be observed. The offline study with saved video is useful in comparison of obtained results via different methods. The recorded track for the same for all experiments. In order to capture an image in a wide area, the camera was placed in a distance of 1.3 m against the marker. In the Fig. 3 the processed image and manipulator’s trajectory were shown. As it was presented in the Fig. 3a, three red markers were used to control an electro-hydraulic manipulator. It simply increases efficient, while only one color must be filtrated. In the bottom right hand corner two markers are visible. Those markers are used to scale pixels from the image to manipulator’s metric units. The last marker is directly used to control the tool center point (TCP) of the manipulator.

Fig. 4
figure 4

Centers of the markers

Full size image

The image in processing application was shown in Fig. 4. A background of image was deleted to extract marker’s features. Three points placed around markers were used to circle fitting algorithm with results in the fitted circles. In the center of markers a results of a center of mass algorithm was presented. In Fig. 5 the output charts from image processing and output of control of the electro-hydraulic manipulator were shown. As it can be noted, in Fig. 3b a fluctuating pixel in X axis in time domain in occurance of disturbances has appeared. In Fig. 5b an angle computed from inverse kinematic was presented. The last, Fig. 6 is the output trajectory from electro-hydraulic manipulator.

Fig. 5
figure 5

Control of electro-hydraulic manipulator: input image from vision system. a Pixels in axis X. b Angle from vision system

Full size image
Fig. 6
figure 6

Control of electro-hydraulic manipulator: angle

Full size image
Fig. 7
figure 7

Position of the TCP of the manipulator. a Position from camera. b Position from manipulator

Full size image

In Fig. 7 absolute positions were compared, these remains the most interesting result. In Fig. 7a the position computed from vision system were presented. First positions were taken from pixels, next inverse kinematic was computed. To check results, forwards kinematic was computed. The fluctuating noise on chart is visible, because the reference signal with scale was taken from two markers which were disrupted by position measurement errors.

The output trajectory from the manipulator was shown in Fig. 7b. The line is more stable, because electro-hydraulic drives are in general inertial objects and cause natural signal filtration.

The overall results were presented in Table 1. As it can be seen, the best results are provide by Kalman filter marked by blue color. A comparison of total amplitude of oscillation in both axes i.e. absolute error was shown in Table 1.

Table 1 Absolute error of position manipulator
Full size table

The total error of positioning of the TCP in electro-hydraulic manipulator was reduced by more than \(40\,\%\).

5 Conclusions

In the article three different methods i.e. circle fitting (proposed by Pratt), central point of mass, and the Kalman filter applied in estimation of a central position of the marker were presented. The first part of the article recalls state of the art. In the second section the testbed and the methods for markers central point estimation were described. To undertake the experimental studies, the research stand was built. The images were captured from camera with frequency up to 50 Hz and the resolution of \(640\,\times \,480\). In the third section the mathematical equations which were used to implementation of the KF were described. In the fourth section the results of the experiment were shown. First part consist of image processing results and electro-hydraulic manipulator trajectory, therefore the charts with angle and the positions of manipulator tip were compared. The main aim of the research was to implement and compare three estimation methods i.e. circle fitting, center of mass and the Kalman filter. All of these methods returns central points of markers. The KF uses two inputs of estimated positions in X and Y axis from other proposed methods. The results prove that KF method is suitable in case of estimation of markers positions and yields better results than CF or CM. However, when input parameters are over the range, the estimation results cannot be valid and the total error can increase. Because errors can appear caused by incompleteness of the markers positions information, all filters are important. The KF method improves the estimation provided by CF or CM method, also by using the KF the output signal has smooth characteristic. What is more, the solution given in the paper ensures good results even though when some image parts are incomplete or require prior reconstruction. The total error in the worst scenario, (\(30\,\%\) of marker is invisible) was reduced by more than 40 %.