應用模糊滑動模式控制之雙眼機械頭強健追蹤控制

(1)

Robust Binocular Tracking Using Fuzzy Sliding Mode Control

計畫編號：NSC 90-2212-E006-127

執行期限：90 年 08月 01日至 91 年 07月 31日主持人：蔡清元 [email protected]

執行單位：國立成功大學機械工程學系中文摘要

傳統在視覺伺服系統中，常使用校正方法量測物

體的深度資訊，然而在應用於追蹤 3D 移動的物體

時，使用校正方法容易產生校正點不易準確求得且具有耗時難以達到即時追蹤等問題。因此，本文將引進超音波感測器來直接量取物體的深度資訊，同時與視覺伺服系統相結合。

在本研究中，我們使用一個影像為基之視覺伺服控制方法，其合併了伺服機構的運動學和影像模型的運動方程式，接著與伺服機構的動力學相結合，以得到一個影像的動力學方程式，然後再加入了模糊滑動模式控制器，另一方面，利用超音波感測器提供目標物深度的資訊給控制器。在影像處理中，我們採用一種快速處理影像的方法，首先先分割影像平面，根據物體的特徵來快速的找到物體的大約位置，接著再建

立了一個注視視窗用於含蓋在3D 移動中的物體。在

視窗內，我們將使用更複雜的影像處理來求出物體的正確位置。另外，引入卡爾曼濾波器來預測目標物的軌跡。

最後，並以一多感測器之雙眼機械頭，進行實驗來驗證理論的推導，實驗內容包括了目標物固定和物體移動的追蹤控制。

關鍵詞：視覺伺服系統、即時追蹤、超音波感測器、

影像為基之視覺伺服控制、模糊滑動模式控制器、注視視窗、卡爾曼濾波器。

Abstract

A calibration method is conventionally used by a visual servo system to calculate the depth of an object.

However, tracking a 3D moving object typically involves problems in calibration, such as an inaccurate calibration point or difficulty in carrying out real-time tracking. This study accordingly uses ultrasonic sensors in conjunction with the visual servo system to directly measure the depth of the object.

This study proposes an image-based visual servo control method, which integrates the kinematics of both the robotic binocular head and cameras with the dynamics of the binocular head to obtain an image-based dynamic equation. The fuzzy sliding mode control algorithm is applied to the image-based dynamic equation using the ultrasonic sensors to directly measure the depth of the object. In image processing, the object

location is first, roughly determined, before more complex image processing is locally performed, to reduce the time taken in the processing. An attention window is estimated to mark the 3D moving object: the size of the window can grow or shrink to follow the size of the object. A Kalman filter is used to predict the trajectory of the object.

Finally, two experiments based on the multiple-sensor binocular head are undertaken to verify the theoretical results. The experiments include active visual tracking of a stationary and moving object.

Keywords: visual servo system, real-time tracking, ultrasonic sensors, image-based visual servo control, fuzzy sliding mode control algorithm, attention window, Kalman filter.

1. Motivation and Objective

Robotic systems have been adopted to execute monotonous, repeated, fixed or dangerous tasks in unchanging environments. Gradual developments in sensor-based robotic systems, such as adding ultrasonic sensors, visual sensors, touch sensors, and others, to the robotic systems, have enhanced the performance of robotic systems and enabled them to react appropriately to sudden environmental changes. Accordingly, robots can replace manpower to execute complicated tasks.

This project employs active vision to provide visual feedback, and utilizes ultrasonic sensors to supply distance information to the visual servo system. This research aims to integrate visual and ultrasonic sensors.

Furthermore, an image-based visual servo controller and a control strategy are proposed for a robotic binocular head to track a target and maintain it at the centers of both image planes of two CCD cameras.

2. Construction of a Ranging Sensor System Ultrasonic sensors, infrared sensors, laser sensors, and CCD cameras are commonly used as range finders in robotic environmental exploration. However, laser range sensors are expensive; CCD cameras require a heavy computational burden for image processing, and infrared sensors can only detect close targets. This study employs ranging sensor system of ultrasonic sensors to determine the distance between the target and the eyes of a binocular head. Ultrasonic sensors are low cost, impose a light computational load and support long

(2)

distance measurement. The system is comprised of two dual-ultrasonic sensor modules, each of which includes two ultrasonic sensors.

A dual-ultrasonic sensor module is designed, as shown in Fig. 1, to determine the distance between a target and a CCD camera mounted on a robotic binocular head, since the ultrasonic sensor always detects the distance between the sensor and the target.

The frame of the dual-ultrasonic sensor module is constructed from an acrylic fiber plate. The frame includes three holes. Two ultrasonic sensors are attached to the top and bottom holes of the frame. The module can be assembled on the front of the CCD camera so that the lens of the CCD camera lies on the center hole.

The distance between the target and each ultrasonic sensor can be calculated by measuring the time of flight (TOF) from the ping to the received echo. Figure 2 depicts the geometrical relationship between the module and the target, where d1 is the measured distance between the top ultrasonic sensor and the target; d2 is the measured distance between the bottom ultrasonic sensor and the target; d represents the distance between the camera and the target, and L is the distance between the two ultrasonic sensors. The cosine formula leads to the following two equations.

2 2 2

cos( ) 1

2 d L d

θ = ^{+ −}dL (1)

2 2 2

cos(180 ) 2

2 d L d θ ^{+ −}dL

− = (2) Combining (1) and (2), yields,

2 2

1 2 ( )2

2 d d

d= + − L (3) Therefore, the distance between the camera and the target can be obtained by calculating (3).

3. Derivation of an Image Jacobian Matrix and an Image Inertial Matrix

This section derives an image Jacobian matrix and an image inertial matrix. We assume a pinhole camera model with a frame attached to it. Consider the instant when the camera moves at translational velocities T [ and =T T T_x _y _z]^T angular velocities R [=R R R_x _y _z]^T , while the observed object is stationary, and let [u v^t ^{t T}] represent the resulting image velocity vector of the object target in the image plane.

Thus, the velocity of the image due to the motion of the camera mounted on the binocular head, can be expressed as follows.

J T R

t t v

u v

 

 =  

    

(4) where

2

0 J

0

y y

k k x

x k k k x k x

v

k k y x x

y k k y k k y

s s

f x f s

xy x y

s Z Z f s f s

f y f s s s

y xy x

s Z Z s f f s

   

− − +

   

   

= −  +  − − 

The ^{2 6}^× image Jacobian ^J^v represents the relations

hip between the velocities of the camera and the re sulting image velocities.

Let k_L and k_R be coordinate frames of the left and right cameras of the binocular head, respectively.

Since two cameras are mounted on the binocular head, two sets of mathematical models of velocity of the image are produced as follows.

J T R

L L

L

t k

L t v

L k

u v

 

 =  

   

   

,

J T R

R R

R

t k

R t v

R k

u v

 

 =  

   

   

(5) whereJ_v_Ldenotes the image Jacobian related to the left camera coordinates and J_v_R represents the image Jacobian related to the right camera coordinates.

The translational velocity and angular velocity in (5) of each camera mounted on the binocular head can be expressed as a linear function of joint velocities using the robotic head Jacobian matrix:

T J

J q q R J

L L

L

k kL

k k

k A

   

= =

   

   

 

T J

J q q

R J

R R

R

k kL

k k

k A

   

= =

   

   

 

(6) Jacobian matrices, ^Jk_L and ^Jk_R , denote the robotic Jacobian matrices of the left and right cameras, respectively. The first three row vectors of each Jacobian matrix are associated with the translational velocity, while the last three correspond to the angular velocity.

From (5) and (6), we can define new Jacobian matrices J^L

L

k

v and J^R

R

k

v to be the products of Jacobian matrices J

vLand J

kL, J

vRand J

kR, respectively.

J^k_v_L^L =J J_v_L⋅ _k_L (7) J^k_v_R^R=J_v_R⋅J_k_R (8)

L L

k

Jv represents the relationship between the joint velocities of the binocular head and the resulting image velocity vector [u^t_L v^{t T}_L] in the image plane of the left camera, while J^R

R k

v represents the relationship between the joint velocities of the binocular head and the resulting image velocity vector [u^t_R v_R^{t T}] in the image plane of the right camera. Now, J^L

L k

v and J^R

R k

v can be integrated to be a new image Jacobian matrix J^k_v for the binocular head that represents the relationship between the joint velocities of the binocular head and the resulting integrated velocity vector of the target

[u^t_L v^t_L u_R^t v^{t T}_R] in the image planes of both the left and right cameras as below.

U J q = ⋅^k_v (9) The dynamics of the binocular head can be expressed by

D q H τ τ⋅ + = − _d (10)

(3)

where D represents the manipulator inertia tensor, and the term H accounts for the Coriolis and centrifugal effects and the gravity torque, while τ is the input torque vector, and τ_d denotes the disturbances.

Differentiating (9) with respect to time yields

U J q J q= ⋅ + ⋅^k_v ^k_v (11) Substituting (10) into (11) yields

U J D (τ τ= ⋅^k_v ⁻1 − −_d H) J q+ ⋅ ^k_v (12) From (12), the visual dynamic behavior of the binocular head can be expressed as follows.

1 1

DJ^k_v⁻ ⋅ + = − +U H τ τ _d DJ^k_v⁻ ⋅ ⋅(J q) ^k_v (13) From (13), the image inertial matrix D_I can be defined as

D_I=DJ^k_v⁻1 (14) If the amount of the joints is less than 4, namely

<4

n , a new image feature vector can be defined by using a linear transformation to make the image Jacobian matrix squared. For instance, a new 3×1 image feature

vector may be defined, U=(u_L^t −u_R^t)/ 2 (u_L^t +u^t_R)/ 2 (v^t_L+v_R^t)/ 2^T, to yield a new

3×3 squared image Jacobian matrixJ^k_v for a robotic binocular head with three joints. Then, the inverse of the new 3×3 image Jacobian can be achieved easily.

However, if the robotic head has more than 4 joints, i.e.

>4

n , the pseudo-inverse technique (Moore-Penrose inverse) can be adopted to get an inverse matrix of ^J^kv. 4. Image Processing

In image processing, the target location is first, roughly determined, before more complex image processing is locally performed, to reduce the time taken in the processing. The main idea behind the fast detection of the target is statistical. First, features (such as color, length, width, and shape, etc.) of the target are chosen. Second, the entire image is subdivided by a square. Finally, each square of the grid is matched to obtain the possible location of the target according to the information of the selected features of the target. Once the approximate location of the target is determined, an attention window is estimated to mark the moving target:

the size of the window can grow or shrink to follow the size of the object. When processing an image, the proportion of the area of the object to that of the attention window ideally should maintain constant, to remove unnecessary background effectively and improve robustness during rough processing. Therefore, the window needs to respond to the changing size of the object. However, in practice, two proportion values need to be defined for switching window size.

Figure 3 is a complete image processing flowchart, including the use of thresholding method to capture the feature of the object. The threshold can be determined from the optimal thresholding, and is based on an approximation to the histogram of the whole image,

using the weighted sum of two or more probability densities with normal distribution. Figure 3 shows that the threshold method was used twice - first in sketchy processing of the whole image and second in detailed processing of the attention window.

5. Design of Visual Servo Controller

In this section, we propose a new visual servo controller using the concepts of sliding mode controller to compensate the model uncertainties of the visual servo system and the disturbances. Consider the higher order terms δ(U,Z_k,q) J q = ⋅^k_v in (11) as external disturbance. The visual dynamic equation of the visual servo system can be rewritten as follows.

D U H τ τ_I⋅ + = − _d+D δ_I⋅

(15) Let

1 2

U ( ) [_d t =U t_d( ) U_d ( )t " U_dn( )]t ^T (16) represent a desired path in image space that we wish the binocular head to track. This path is assumed herein to be continuously differentiable. For the problem of tracking the desired trajectory (16), we form the visual position error vector

e U U= − d (17) Let the estimated visual dynamic model of the visual servo system be

δ Dˆ τ τ Hˆ U

Dˆ_I⋅+ = − _d+ _I⋅

(18) Given the dynamic equation (15) and the available model (18), a sliding control scheme is implemented with the following form

Hˆ u Dˆ

τ = I⋅ + (19) Substitute (19) into (15), yields

1ˆ 1 1 1

U D D u D= _I⁻ _I⋅ + _I⁻⋅ ∆ −H D τ_I⁻ _d+D D δ_I⁻ _I⋅ (20) where ∆ = −H H H^ˆ . Furthermore, two sets of n-vectors,

L_j and ∆D_I_j (j=1,",n) can be defined by

1

1 2

D_I⁻ =L L " L_n^T

1 2

D_I Dˆ_I D_I  D_I D_I D_I_n

∆ = − = ∆ ∆ " ∆  Equation (20) can then be simplified as

1 1 1

U u D= + _I⁻∆ ⋅ +D u D_I _I⁻ ∆ −H D τ_I⁻ _d+δ

(21) From (21), the i-th term of U can be expressed as

1L D L H L τ

j

n T T T

i i i I j i i d i

j

U u u δ

= +∑= ∆ + ∆ − +

(22)

If the sliding surface is selected for the i-th input as follows:

d τ τ e e

e

s_i = _i + 2λ_i _i + λ_i²∫₀^t _i( ) (23) where e_i=U_i−U_di. The control objective is to keep the target in the centers of two image planes, that is,

=0

= di

di U

U

(24) Differentiate (23) with respect to time, yields

i i i i i d T

i T

i j I n j

T i i

i i i i i i

e e δ u

u

e e U s

j

2 2

λ λ τ

L H L D L

λ λ

+ + +

−

∆ +

∆ +∑

=

+ +

=

2 2

1

(25)

(4)

The components u_i ( i=1,",n ) of vector u are defined as:

i i eq

i u u

u = +∆ (26) where λ_i _i λ_i _i λ_i _i λ_i( _i _di)

eqi e e U U U

u =−2 − ² =−2 − ² − . Then, (25) can be rewritten as follows.

(

^{1 L} ^Di

)

^L ^Dj j ^L ^Dj ^L ^{H L τ}

T T T T T

i i I i i I eq i I j i i d i

j j i

s u u u δ

= + ∆ ∆ +∑ ∆ +∑≠ ∆ ∆ + ∆ − +

(27) Neglecting the effect of L^T_i D_I_j _j

j i u

≠ ∆ ∆

∑ , and

assuming L^T_i∆D_I_i ≤ ∆ <_i 1 , we can obtain

1 1 L D L H L τ

1 L D_i 1 L D_i ^j ^j

T T T

i i i i i I eq i i d i i

T T

i I i I j

s s = ∆u s+ ∑ ∆ u + ∆ − +δ s

+ ∆ + ∆  

(28) Therefore, if one part of the control law, ∆u_i, is

designed to satisfy the sliding condition, that is, (28) is less than zero, the desired path (16) can be achieved asymptotically. With the definition of (26), the control law for tracking the target can be written as

(

^u ^u

)

^H^ˆ

{

^D^ˆ ^u ^H^ˆ

}

^Dˆ ^u

Dˆ

τ = I eq+∆ + = I eq+ + I∆ (29) where

1 2

u_eq=u_eq u_eq " u_eq_n^T，^{∆ = ∆}û ^ û¹ ^∆û² " ^∆ûⁿ^^T Fig. 4 illustrates the control structure of the visual servo system. A fuzzy logic controller is designed within the control structure to force the system dynamics to approach the sliding surface and stay on the surface in the presence of model uncertainties and disturbances.

Fuzzy logic control focuses mainly on fuzzifying the sliding surface S(X,t)=0 to solve the chattering problem. According to Fig. 4, one part of the control law,

∆u , is determined by the fuzzy logic controller.

Meanwhile, the rules and parameters of the fuzzy logic controller can be determined by the sliding mode control.

When implementing the fuzzy logic controller, s_i can be approximated by

) ( ) ( )

( = − −1

∆s_i k s_i k s_i k (30) where k denotes a positive integer. According to Fig. 5, the i-th input of the fuzzy logic controller includes

) (k

s_i and ∆s_i(k) , and its output is ∆u_i(k) . The fuzzifized variables of s_i(k), ∆s_i(k), and ∆u_i(k) are normalized to s′_i , ∆s′_i , and ∆u′_i , respectively.

Consequently, the range of non-fuzzy variables s_i(k), )

(k s_i

∆ , and ∆u_i(k) must be scaled to fit the universe of discourse of fuzzifized variables s′i , ∆s′i and ∆u′i

with scaling factors of k₁, k₂ and k₃, respectively.

Each fuzzy variable is quantified into seven qualitative fuzzy variables: <1>PB: Positive big, <2>PM:

Positive medium, <3>PS: Positive small, <4>ZO: Zero,

<5>NS: Negative small, <6>NM: Negative medium and

<7> NB: Negative big. Figure 6 presents the membership function. Since only the sign of s _is_i is concerned in the sliding condition, according to (28), (31)

is used herein as the sliding condition in the implementation.

1 L D L H L τ 0

1 L D_i ^j ^j

T T T

i i i i Ti I j i I eq i i d i i

s s= ∆u s+ + ∆ ∑ ∆ u + ∆ − +δ s < (31) According to (31), the fuzzy control rules for the fuzzy logic controller can be obtained and displayed in Table 1.

To reduce the on-line computation time, Mamdani’s

“sup-min” compositional operator and “min” fuzzy implication and the generalized center of area method are used to form a look-up table for the output ∆u_i′(k)of the fuzzy logic controller. Then, the control law (29) of the sliding mode based fuzzy control can ensure the robotic head to trace the target and keep it at the centers of both image planes.

6. Control Structure of the Visual Tracking System

The section proposes a control strategy for a robotic binocular head to track a target and maintain it at the centers of both image planes of two CCD cameras. As the control structure illustrated in Fig. 7, the whole system is composed of the binocular head subsystem, the image processing subsystem, and the dual-ultrasonic sensor module subsystem. The binocular head subsystem is controlled by a visual fuzzy sliding mode controller concerned not only with the kinematics but also the dynamics of the subsystem, as described in Section 5.

The image processing subsystem aims to obtain image information, and the dual-ultrasonic sensor module subsystem measures the distance between the CCD cameras and the target.

The image feature of a moving object is extracted from its captured image as the target for visual tracking.

Following the achievement of the position of the target in the image plane, a Kalman filter is introduced to predict the next position of the target, to compensate for the time delay in image processing. While the predicted velocity of the target is derived from the predicted position obtained by the Kalman filter, using a second-order Taylor series expansion (TSE) estimator.

Both the predicted position and velocity of the target are then fed back to the visual servo controller. During visual tracking, the distance information between CCD cameras and the target is mainly provided to the visual fuzzy sliding mode controller by the dual-ultrasonic sensor module subsystem. To avoid the violent change of the distance measurement due to the interference from the environment, a safety mode is established so that the image processing subsystem can provide the approximate distance information based on the size of the attention window.

7. Experimentation

An experimental environment is established to verify the theory. The experimental environment includes a robotic binocular head and a blackened

(5)

ping-pong ball to be tracked. A binocular head equipped with a pair of dual-ultrasonic sensor modules was constructed in the Robotic Systems and Control Lab., National Cheng Kung Univ., as illustrated in Fig. 8.

Figure 9 shows how the binocular head, the visual subsystem, the control subsystem, and the dual-ultrasonic sensor module subsystem are integrated into an active robotic visual tracking system. The DSP network control subsystem can be established outside the host PC, communicating with the host PC through a PCI Bus. It can simply be embedded into the host PC if fewer cards than PCI expansion slots are available. Here, the DSP network control system was embedded in the host PC and can parallel-process the control strategy of the binocular head. The visual subsystem is responsible for the simultaneous acquisition of images from two CCD cameras. The image is then transferred to the host computer via a PMC. The dual-ultrasonic sensor module subsystem communicates measured distances to a host PC via the local Internet.

A thick wire is set across the space in front of the binocular head. The ball is placed on a hook, which is hung over the thick wire. A thin wire is set close and parallel to the thick wire. One end of the thin wire is connected to the hook, and another end to a small motor to drive the ball along the thick wire so that the ball can move in a straight line. Figure 10 depicts the simple positional relationship. As shown in Fig. 10, the binocular head is at its home position. The coordinates

( 20− cm, 20− cm,7.3cm) and (20cm, 20− cm,7.3cm) specify the position of left and right CCD Cameras relative to the base coordinate frame of the binocular head. The coordinate (0cm cm,0 ,67.3cm) is the initial position of the ball and (58cm, 32− cm,112.3cm) is its final position relative to the base coordinate frame of the binocular head.

Two experiments are performed in this research.

Since the binocular head does not saccade the surrounding environment to find the target, the first joint of the head is locked during the experiments. In the first experiment, the ball is placed at its initial position and the binocular head starts the fixation on the ball from its home position. Figures 11 and 12 show the fixation of both cameras, the purpose of which is to locate the target in the center of the left and right image planes. Then, in the second experiment the binocular head tracks the ball moving from its initial position to its final position after the target is fixated in the centers of the left and the right image planes in the first experiment. The target moves about 80cm in 25 sec, as shown in Fig 10. It moves from (0cm cm,0 ,67.3cm) to (58cm, 32− cm,112.3cm). When the target is about 70 cm away from the starting position, an irregular object abruptly bursts into the range of the ultrasonic wave and stays there but it does not cover the image of the target. The average velocity of the target is 3.2 cm/s. When the target moves, it is tracked by the binocular head. This behavior is called “gaze control”, because the target is tracked so that it is at the centers of

both image planes. Figures 13 and 14 give the distance measured by the dual-ultrasonic sensor modules. We can find there is a violent change of the distance measurement at around 22 sec due to the interference from the foreign object. At that time, the attention window can provide approximate distance information to the visual servo controller. Figures 15 and 16 show the binocular head gazes at the center of a moving ball.

8. Conclusion

This research integrates three subsystems, including the binocular head, the image processing, and the dual-ultrasonic sensor module.

In the image processing subsystem, the object location is first, roughly determined, before more complex image processing is locally performed, to reduce the time taken in the processing. An “attention window” is estimated to mark the 3D moving object: the size of the window can grow or shrink to follow the size of the object.

This study further proposes an image-based visual servo control method, combining the dynamics of the binocular head and the kinematics of both the cameras and binocular head, and using the fuzzy sliding mode control algorithm. The Kalman filter is adopted to predict an unknown target trajectory.

Finally, the dual-ultrasonic sensor module measures the distance between the CCD Camera and the object.

However, the distance measurement causes a large error if a foreign object abruptly bursts into the range of the ultrasonic wave. The attention window can offer approximate distance information to prevent this problem.

9. References

[1] A. Bernardino and J. Santos-Victor, “Binocular tracking: integrating perception and control,”

Robotics and Automation, IEEE Transactions on, Vol.15, pp. 1080–1094, 1999.

[2] P. Corke, “Design, Delay and Performance in Gaze Control: Engineering and Biological Approaches,”

The Confluence of Vision and Control, pp.146-158, 1998.

[3] Seth Hutchinson, Gregory D. Hager, and Peter I.

Corke, “A Tutorial on Visual Servo Control,” IEEE Transactions on Robotics and Automation, vol. 12, pp. 651-670, 1996.

[4] Kai-Tai Song and Wen-Hui Tang, “Environment Perception for a Mobile Robot Using Double Ultrasonic Sensors and a CCD Camera,” IEEE Transactions on Industrial Electronics, Vol. 43, No.

3, pp.372 -379, 1996.

[5] T. I. James Tsay and Jiann-Hwa Huang, 1994,

“Robust Nonlinear Control of Robot Manipulators”, Proceedings of the 1994 IEEE International Conference on Robotics and Automation, pp.

2083-2088.

[6] G.-Q. Wei , K. Arbter, and G. Hirzinger , “Active

(6)

self-calibration of Hand Cameras and Hand-eye Relationships with motion planning,” IEEE International Conference on Robotics and Automation, Vol .2, pp. 1359 -1364, 1997.

[7] Z. Zhang, “On the epipolar geometry between two images with lens distortion,” Proceedings of the 13th International Conference on, pp. 407 – 411, 1996.

[8] “Daytona Dual ‘C6x PCI Board Technical Reference,” Technical Report, Spectrum corp., 1999.

Ultrasonic Sensor (Transducer)

Fig. 1 Schematic of the dual-ultrasonic sensor module

Fig. 2 Geometrical relationship of the module to the target

Local Threshold :T2 Globel Threshold:T1 Determine Attention

Window Size

Segment Whole Image to a N x N Grid of Squares with

n x n pixels

Fast Detect Object Location by Attention Window

3 x 3 Average Filter h

Calculate Area of Object

Determine Center of Object Capture Full Image

3 x 3 Convolution Mask G Proportion Values for

Switching Window Sizes

Fig. 3 Complete image processing flowchart

FLC

τ

S ++

-- +

∆u ueq

0 U U

d d

=

U

u

U

2λ_iU_i λ_i2e_i

− −

∧

∧u+H DI

Image Feature Extractor Kalman

Filter

Robotic Binocular Head Image

Flow Estimator

+

2 0

2 ^t ( )

i i i i i

U+λe+λ∫eτ τd

Fig. 4 Control structure of the visual servo system

Fuzzy Control Rule

&

Fuzzy Inference k1

k2

k3 '

si

∆ si

si

∆

ui

∆

'

ui

∆

'

si

Fig. 5 Fuzzy logic control structure for the i-th input

' '

' , ,

i i

i s u

s µ∆ µ∆

µ

'

si

∆

'

si

'

ui

0 ∆ 1

1

−1 −2/3 −1/3 1/3 2/3

ZO PS PM PB

NS NB NM

Fig. 6 Membership function

Kalman Filter

Visual FSMC

Binocular Head

Image Capture Image

Preprocessing Image

Flow Estimator

Desired Motion in the

Image Plant +

Dual-Ultrasonic Sensor Module Subsystem Image Feature of

Target

-

Image Processing Subsystem

Binocular Head Subsystem Safety_Mode

Switch U

U

+

Fig. 7 Control structure of the visual tracking system

Fig. 8 The binocular head equipped with a pair of dual-ultrasonic sensor modules

Fig. 9 Hardware architecture of the experimental setup

(7)

(0,0,67.3) (58,-32,112.3)

(0,0,0) XBase Final

Ball

Inital

Ball

ZBase

YBase

(20,-20,7.3) XkR

ZkR

YkR

(-20,-20,7.3) XkL

ZkL

YkL

Fig. 10 Positional relationship for the binocular head and ball

0 40 80 120 160 200

x coordinate (pixel) 0

40 80 120 160 200

y coordinate (pixel)

(99,102)

(148,22)

Fig. 11 Tracking trajectory of a stationary target in the left image plane

0 40 80 120 160 200

40 80 120 160 200

(101,102)

(24,22)

Fig. 12 Tracking trajectory of a stationary target in the right image plane

5 10 15 20 25

Time (sec) 60

80 100 120 140

Distance (cm)

Fig. 13 Distance between target and Left-CCD Camera by dual-ultrasonic sensor module

5 10 15 20 25

Time (sec) 60

80 100 120 140

Distance (cm)

Fig. 14 Distance between target and Right-CCD Camera by dual-ultrasonic sensor module

0 40 80 120 160 200

40 80 120 160 200

Fig. 15 Tracking trajectory of a moving target in the left image plane

0 40 80 120 160 200

40 80 120 160 200

Fig. 16 Tracking trajectory of a moving target in the right image plane

Table 1 Rule table of the fuzzy logic controller

∆s i′

∆u i′

NB NM NS ZO PS PM PB NB PB PB PB PB PM PS ZO NM PB PB PB PM PS ZO NS

NS PB PB PM PS ZO NS NM ZO PB PM PS ZO NS NM NB PS PM PS ZO NS NM NB NB PM PS ZO NS NM NB NB NB

i′ s

PB ZO NS NM NB NB NB NB