Passivity-Based Visual Motion Observer Integrating Three Dimensional Target Motion Models

Transcription

1 SICE Journal of Control, Measurement, and System Integration, Vol. 5, No. 5, pp , September 2012 Passivity-Based Visual Motion Observer Integrating Three Dimensional Target Motion Models Takeshi HATANAKA and Masayuki FUJITA Abstract : This paper investigates a vision-based 3D rigid-body motion estimation problem. In one of our previous works, the authors addressed the problem using no prior information on the target motion. On the other hand, this paper presents another approach assuming some target motion patterns. The authors first consider a constant velocity model, which is a typical choice of motion patterns, and present a novel motion observer integrating the motion model. It is then proved based on passivity that the presented observer leads both of the estimates of the target object pose and body velocity to their actual values. Moreover, the result is extended to a more general motion pattern. Finally, the effectiveness of the presented estimation mechanism is demonstrated through experiments. Key Words : vision-based motion estimation, rigid-body motion, passivity. 1. Introduction A large amount of literature has devoted to fusion of control theory and computer vision [1] [6] mainly motivated by robot control. Currently, the motivating scenarios of the fusion spread over robotic systems into security and surveillance systems, medical imaging procedures and even understanding biological perceptual information processing. The history and recent developments are well summarized in [7] [9]. This paper focuses on vision-based 3D target object motion estimation of a moving target as in [10] [12]. The papers [10],[11] address a problem called structure and motion from motion via estimation methods in systems and control theory. The paper [12] investigates a 3D pose estimation/control problem for a moving target object based on passivity of rigid-body motion, where a vision-based observer called visual motion observer plays a central role. Here, the authors analyze a tracking performance of a camera to a moving object in the framework of L 2 -gain regarding the target velocity as unknown disturbances. As another way to deal with the target velocity, assuming some target object motion pattern is commonly used in visual servoing [1],[7],[8] and visual tracking [9] in order to cancel the tracking errors. A typical selection of the target motion patterns is a constant velocity model [8],[9],[13] inspired by a classical control theory to cancel tracking errors through an integral term. As the other option, a constant acceleration model is also employed in [13]. Periodic motion is also useful especially in medical robotics in order to model the heartbeat and breathing [14]. This paper investigates the problem of [12] assuming target motion patterns as prior knowledge. In particular, a novel motion observer integrating the 3D target motion model is proposed. The authors first deal with a constant velocity model Department of Mechanical and Control Engineering, Tokyo Institute of Technology, S5-26 Ookayama, Meguro-ku, Tokyo , Japan hatanaka@ctrl.titech.ac.jp (Received February 8, 2012) Fig. 1 Configurations of vision camera and target object. and prove that the presented observer leads both of the target pose estimate and body velocity estimate to their actual values. Moreover, the authors also present its extension to a generalized motion model and also prove the same statement as the constant velocity case. Finally, the effectiveness of the presented estimation mechanism is demonstrated through experiments on a testbed. 2. Rigid Body Motion and Measurements 2.1 Relative Rigid Body Motion Let us now consider the situation where a target object and a vision camera are in 3-D space as depicted in Fig. 1, where the coordinate frames Σ w, Σ c and Σ o represent the world frame, the vision camera frame, and the object frame, respectively. The position vector and the rotation matrix from the camera frame Σ c to the world frame Σ w are denoted by p wc R 3 and eˆξ wc θ wc SO(3) := {R R 3 3 R T R = I 3 and det(r) =+1}. The vector ξ wc R 3 specifies the rotation axis and θ wc Ris the rotation angle. For simplicity, we use ξθ wc to denote ξ wc θ wc.the notation is the operator such that âb = a b, a, b R 3 for the vector cross-product,i.e.âis a 3 3 skew-symmetric matrix. The vector space of all 3 3 skew-symmetric matrices is denoted by so(3). The notation denotes the inverse operator to. The pair of the position p wc and the orientation eˆξθ wc denoted by g wc = (p wc, eˆξθ wc ) SE(3) := R 3 SO(3) is called JCMSI 0005/12/ c 2012 SICE

2 SICE JCMSI, Vol. 5, No. 5, September Fig. 3 Block diagram of the camera model with RRBM. Fig. 2 A simple vision camera model. a pose of camera relative to Σ w. Similarly, we denote by g wo = (p wo, eˆξθ wo ) SE(3) pose of the object relative to the world frame Σ w. We also define the body velocity of the camera relative to the world frame Σ w as Vwc b = (v wc,ω wc ) R 6,wherev wc and ω wc respectively represent the linear and angular velocities of the origin of Σ c relative to Σ w [2]. Similarly, object s body velocity relative to Σ w is denoted as Vwo b = (v wo,ω wo ) R 6. In this paper, we use the following homogeneous representation of g = (p, eˆξθ ) SE(3) and V b = (v,ω). g = [ eˆξθ p 0 1 ], ˆV = [ ˆω v 0 0 Then, the body velocities Vwc b and Vwo b are simply given by ˆV wc b = g 1 wcġ wc and ˆV wo b = g 1 woġ wo. Let g co = (p co, eˆξθ co ) SE(3) be the pose of Σ o relative to Σ c. Then, it is known that g co can be represented as g co = g 1 wcg wo. By using the body velocities Vwc b and Vwo, b the motion of the relative pose g co is written as ġ co = ˆV b wcg co + g co ˆV b wo. (1) Equation (1) is a standard formula for the relation between the body velocities of three coordinate frames [2]. 2.2 Visual Measurement In this subsection, we define the visual measurement of the vision camera which is available for estimation of object motion. Throughout this paper, we use a pinhole camera model with a perspective projection [2] (Fig. 2). We assume that the object has m feature points (m 4) and the vision camera can extract them from the visual data. Let p oi R 3 and p ci R 3 be the position vectors of the object s i-th feature point relative to Σ o and Σ c, respectively. Using a transformation of the coordinates, we have p ci = g co p oi where p ci and p oi should be regarded, with a slight abuse of notation, as [p T ci 1]T and [p T oi 1]T. Let the feature points on the image plane coordinate f := [ f1 T fm T ] T R 2m be the visual measurement of the camera. Then, it is well known [2] that f i is given by the perspective projection f i = λ xci, p z ci y ci = [x ci y ci z ci ] T, (2) ci where λ is a focal length of the camera. In this paper, we assume that the point features p oi R 3 are known a priori. Then, the vision data vector f (g co ) depends only on the relative pose g co. Figure 3 illustrates the block diagram of the relative ]. rigid body motion with the camera model, where RRBM is an acronym for Relative Rigid Body Motion. Under the situation, Fujita and others [12] present a motion estimator called visual motion observer to estimate g co from the visual measurement f. Moreover, they analyze estimation accuracy in the framework of L 2 gain analysis regarding the body velocity V b wo as unknown disturbances. On the other hand, this paper presents another approach to the problem assuming that a pattern of the target motion is known a priori and available for estimation. This assumption is valid in case where we have some prior knowledge on the target object motion and, moreover, such a model might be obtained from the past profiles of f. 3. Constant Velocity Model In this section, we assume that the target object has a constant body velocity V b wo = c and that its model V b wo = 0 (3) is available for estimation. Then, the objective here is to present an observer producing the correct estimates of the relative pose g co and object body velocity V b wo. Note that (3) is useful not only in the case where the velocity is really constant since any signal is approximated by a piecewise step function. 3.1 Target Motion Model and Estimation Error Similarly to [12], we first prepare a model of the target object motion not only for (1) but also for (3) as V wo b = u v, (4a) ḡ co = ˆV wcḡ b co + ḡ co ˆ V wo b ḡ co û e, (4b) where V wo b = ( v wo, ω wo ) is the estimate of Vwo b and ḡ co = ( p co, eˆ ξ θ co ) is the estimate of g co. The inputs u v = (u vv, u vω )and u e = (u ep, u er ) are to be determined in order to drive the estimated value ( V wo, b ḡ co ) to their actual value (Vwo, b g co ). Note that once the pose estimate ḡ co is determined, the estimated measurement f = ( f 1,, f m ) is also computed from (2) by using the estimate ḡ co instead of g co. Let us now define the estimation error between the estimated value ḡ co and the actual g co as g e = (p e, eˆξθ e ):= ḡ 1 co g co. Using the notations e (eˆξθ R ):= sk(eˆξθ ), sk(eˆξθ ):= 1 (eˆξθ 2 e ˆξθ ), we also define the vector of the estimation error e e := (p e, e R (eˆξθ e )). If the vector e e is equal to zero, then the estimated relative pose ḡ co equals the actual g co. Let us now define the visual measurement error f e as f e := f f. Then, it is shown in [12] that the estimation error vector e e is approximately reconstructed by e e = J (ḡ co ) f e, (5) where denotes the pseudo-inverse and J( ) :SE(3) R 2m 6 is the well-known image Jacobian [2]. It is also known that if m 4 the image Jacobian has the full column rank.

3 278 SICE JCMSI, Vol. 5, No. 5, September 2012 Fig. 4 Estimation error system with target motion model. Fig. 5 Estimation error system with velocity loop. We next define the estimation error between the actual body velocity V b wo and its estimate V b wo as V e := V b wo V b wo. Then, the time evolutions of the estimation errors V e and g e are formulated as V e = u v, ġ e = û e g e ˆ V b wog e + g e ˆV b wo. (6a) (6b) Note that if V b wo = 0, (6b) is equivalent to the evolution of the estimation error g e in [12]. 3.2 Estimation Error System In this section, we consider an estimation error system from input (u v, u e ) to output (V e, e e ) whose block diagram is illustrated in Fig. 4. Since (6b) is not fully described as a function of g e and V e, we choose x = (Vwo, b V e, g e ) as a state variable of the estimation error system. Accordingly, the state equations are given by (3) and (6). Thegoalistodesign(u v, u e ) so that lim t (V e, e e ) = 0, which is equivalent to convergence of the estimates ḡ co and V wo b to their actual values g co and Vwo. b Namely, the problem is reduced to an output regulation problem leading the output (V e, e e ) to zero for the estimation error system (3) and (6). Note that the control objective has to be achieved by using only the estimation error vector e e reconstructed from visual measurement f via (5). Let us now close the loop of u v with a negative feedback u v = k v e e, k v > 0 (7) in advance and we will design only u e in the next subsection so as to achieve lim t (V e, e e ) = 0. The meaning of (7) will be explained later. The block diagram of the estimation error system with (7) is illustrated in Fig. 5. Then, the state equation (3) and (6) with the inner loop (7) is divided into the orientation part ω wo = 0, ω e = k v e R (eˆξθ e ), (8a) (8b) ėˆξθ e = û er eˆξθ e ˆ ω wo eˆξθ e + eˆξθe ˆω wo, (8c) and the position part v wo = 0, v e = k v p e, ṗ e = eˆξθ e v wo ( ˆ ω wo + û er )p e v wo + u ep. (9a) (9b) (9c) Note that (9) depends on (8) while (8) is independent of (9). We thus first consider only the evolution of orientation estimates and then deal with the position part. 3.3 Stabilization of Orientation Error System Let us first consider the system described by (8), whose input is u er, controlled output is (ω e, e R (eˆξθ e )) and measured output is e R (eˆξθ e ). For notational simplicity, we denote the state by x R := (ω wo,ω e, eˆξθ e ). Then, we have the following lemma. Lemma 1 The system (8) is passive from u er to e R (eˆξθ e ) with the storage function U R (x R ) = φ(eˆξθ e )+S ω (ω e ), where φ(eˆξθ e ) = 1 2 tr(i 3 eˆξθ e )ands ω (ω e ) = 1 2k v ω e 2. Proof Similarly to [12], the time derivative of the energy function φ(eˆξθ e ) along with (8c) is given by φ(eˆξθ e ) = e T R (eˆξθ e ) ( ) e ˆξθ e (u er ω wo ) + ω wo = e T R (eˆξθ e ) ( ) u er ω wo + ω wo = e T R (eˆξθ e )u er + e T R (eˆξθ e )ω e. (10) In addition, it is straightforward that the time derivative of S ω along with (8b) is given by Ṡ ω = e T R (eˆξθ e )ω e. (11) From (10) and (11), we have U R = u T er e e R(eˆξθ ). This completes the proof. Lemma 1 means that the negative feedback u er = k e e R (eˆξθ e ) (12) makes the energy U R non-increasing and hence we can prove the following theorem. Lemma 2 Consider the system (8) with (12). Then, lim t (e R (eˆξθ e ),ω e ) = 0 holds and hence both of the orientation and angular body velocity estimates converge to their actual values. Proof We prove the lemma using LaSalle invariance principle [15]. Since the energy U R is non-increasing and the angular velocity ω wo is constant, the set Ω := {x R U R (x R ) U R (x R (0)) and ω wo = ω wo (0)} is a positively invariant set of (8) with (12). From the definition of U R,thesetΩis compact. Hence, LaSalle invariance principle implies that all the state trajectories asymptotically converge to the largest invariant set contained in E := {x R U R = 0} = {x R e R (eˆξθ e ) = 0}. InthesetE, eˆξθ e = I 3, e R (eˆξθ e ) = 0and ėˆξθ e = 0 hold. Substituting these equations into (8c) yields 0 = ėˆξθ e = ˆ ω wo + ˆω wo = ˆω e.

4 Namely, the only solution that can stay identically in E is the trivial solution ω wo ω wo (0), (e R (eˆξθ e ),ω e ) 0. Thus, all the state trajectories asymptotically converge to the set of states satisfying (e R (eˆξθ e ),ω e ) = 0. Unlike the paper [12] with V b wo = 0, the system (8c) describing evolution of the orientation estimation error eˆξθ e is not passive due to the second term of (10). The term is canceled by the inner velocity loop (7). Hence, the operation (7) is interpreted as a kind of passivation of the orientation estimation error system. SICE JCMSI, Vol. 5, No. 5, September Stabilization of Position Error System The goal of this subsection is to design the input u ep so that the estimation errors p e and v e following (9) converge to 0. For this purpose, we first present the input u ep = ( ˆ ω wo k e I 3 )p e. (13) Substituting (12) and (13) into (9) yields v e = k v p e, ṗ e = eˆξθ e v wo sk(eˆξθ e )p e v wo k e p e. (14a) (14b) Using the notation x p = (v e, p e ), (14) is rewritten as 0 kv I ẋ p = Φx p + ψ, Φ := 3, (15a) I 3 k e I 3 [ ] 0 ψ := (I 3 eˆξθ e )v wo + sk(i 3 eˆξθ e. (15b) )p e Let us now view (15) as a linear system with a perturbation ψ. Note that the perturbation ψ is bounded as ψ 2 I 3 eˆξθ e 2 v wo 2 + I 3 eˆξθ e 2 p e 2 I 3 eˆξθ e F v wo 2 + I 3 eˆξθ e F x p 2 = γ(t) x p + δ(t), (16) γ(t) := 2 φ(eˆξθ e), δ(t) := 2 φ(eˆξθ e) vwo 2, where x 2 is the 2-norm of a vector x, and M 2 and M F for amatrixm are the induced 2-norm and the Frobenius norm of M, respectively. We are ready to prove the main result of this section. Theorem 1 Consider the estimation error system (6) for a constant velocity model (3) with the input u v = k v e e, u e = K e e e, (17) ke I K e := 3 ˆ ω wo 0. (18) 0 k e I 3 Then, we have lim t (V e, e e ) = 0. Proof It is sufficient from Lemma 2 to prove lim t (p e, v e ) = 0. The nominal system (15) with ψ = 0 is stable since the real parts of all the eigenvalues of Φ are negative as long as k e > 0 and k v > 0. From linearity of the nominal system, we can also conclude that the nominal system is exponentially stable. In addition, it is immediate to confirm that both of γ and δ are vanishing from Lemma 2. Thus, just applying Lemma 9.6 of [15] to the system completes the proof. Fig. 6 Visual motion observer with target motion model. The above procedure of the proof implies that the energy of the position error U p and hence U p +U R also can increase in the presence of the orientation estimation error. This fact prohibits a direct application of the Lyapunov method, which is different from the case of V b wo = 0. The total estimation mechanism is formulated as VMO with TMM : V wo b = u v (4a) ḡ co = ˆV wcḡ b co + ḡ co ( ˆ V wo b û e ) (4b) e e = J (ḡ) f e (5) u v = k v e e, u e = K e e e, (17) where VMO and TMM are respectively the acronyms for Visual Motion Observer and Target Motion Model. The resulting observer system is depicted in Fig. 6. It should be noted that if u e := V b wo u e is newly viewed as the input of the observer, then the input u e = k v e I + K e e e, ė I = e e (19) has the same structure as a proportional-integral controller, which is natural from internal model principle. 4. Generalized Motion Model 4.1 Generalized Target Motion We assume that the target object body velocity Vwo b is given in the form of a finite Fourier series expansion V b wo= c + n a i sin w i t + b i cos w i t, (20) i=1 where a i = (a v,i, a ω,i ) R 6, b i = (b v,i, b ω,i ) R 6, c = (c v, c ω ) R 6, and the frequencies w i, i = 1,, n are known a priori. Clearly, the model includes that in Section 3 as a special case. Let us now define z v,0 = c v, z v,i = a v,i sin w i t + b v,i cos w i t R 3, z ω,0 = c ω, z ω,i = a ω,i sin w i t + b ω,i cos w i t R 3, z v = (z v,1,, z v,n ), z ω = (z ω,1,, z ω,n ) R 3n x := (z v,0, z v, ż v, z ω,0, z ω, ż ω ) R 6+12n. Then, it is straightforward to see that the time evolution of V b wo is represented by the linear time invariant system,

5 280 Av 0 ẋ = Ax, A :=, (21a) 0 A ω Vwo b = Cx, C := C v C ω (21b) A v = A ω := 0 0 I 3n 0 diag(w 2 1,, w2 n) I 3 0 C v = C ω := [ 1 T n+1 I 3 0 ], 1 n = (1,, 1) R n. We also have the following lemma. Lemma 3 Let B ω = Cω. T Then, the system (A ω, B ω, C ω, 0) with state x ω is passive with respect to the storage function S ω := 1 2 xt ωpx ω with I3(n+1) 0 P := 0 diag(1/w 2 1,,. 1/w2 n) I 3 Proof Let us denote the input of the system by u. Then, the time derivative of S ω along with (24) is given as follows. Ṡ ω = xωp(a T ω x ω + B ω u) = xω T 0 0 I 3n x ω + xωb T v u = (C ω x ω ) T u 0 I 3n 0 This completes the proof. Remark that the model (20) is useful even if V b wo is not really periodic, since a future profile of V b wo over a finite interval is approximated as (20) to an arbitrary accuracy. Namely, we can regard the estimation process over the infinite time interval as repeats of the estimation over a finite time interval. Of course, a variety of real periodic motion is approximately described in the form of (20). 4.2 Estimation Error System We first build a model of (21) as x = A x Bu v, B = (B v, B ω ):= C T V wo b = C x, SICE JCMSI, Vol. 5, No. 5, September 2012 (22a) (22b) together with (4b), where x = ( x v, x ω )and V b wo = ( v wo, ω wo )are the estimates of x and V b wo respectively. Now, we define the estimation errors x e = (x ev, x eω ):= x x and V e = (v e,ω e ):= V b wo V b wo. Then, the evolution of x e is formulated by ẋ e = Ax e + Bu v, V e = Cx e. (23a) (23b) Namely, the state equation of the estimation error system is given by (6b) and (23). Note that the evolution of x e = (x eω,ω e ) follows ẋ eω = A ω x eω + B ω u vω, ω e = C ω x eω, (24a) (24b) which is passive with the storage function S ω (Lemma 3). In addition, the evolution of (x ev, v e ) is formulated as ẋ ev = A v x ev + B v u vv, v e = C v x ev. (25a) (25b) We finally close the loop of u v and u e by the same input (17) as Section 3. Then, the total estimation mechanism is formulated as VMO with TMM : x = A x Bu v, V wo b = C x, (22) ḡ co = ˆV wcḡ b co + ḡ co ( ˆ V wo b û e ) (4b) e e = J (ḡ) f e (5) u v = k v e e, u e = K e e e, (17) In the next section, we will prove that the estimation mechanism correctly estimates g co and V b wo. 4.3 Convergence Analysis This subsection proves the following theorem. Theorem 2 Consider the estimation error system (6b) and (23) with the input (17). Then, all the state trajectories of the system satisfy lim t (V e, e e ) = 0. In the proof of Theorem 2, we use the following lemma. Lemma 4 The following matrix is stable. Av k Γ := v B v C v k e I 3, (26) Proof Let (y 0,, y 2n+1 ), y i R 3 be an eigenvector of Γ corresponding to an eigenvalue λ. Then, from the definition of Γ, we get the equations (λ + k e )y 2n+1 = n y i, k v y 2n+1 = λy 0 (27) i=0 k v y 2n+1 = y n+i λy i,λy n+i = w 2 i y i, i [1, n] (28) From (28), we have y i = k vλ y λ 2 + w 2 2n+1. (29) i Substituting the second equation of (27) and (29) into the first equation of (27) yields λ + k e = k n v λ k v λ. λ 2 + w 2 i i=1 We denote λ = λ 1 + 1λ 2. Then, by comparing the coefficients of the real part, we have ( 1 n λ i ) λ 1 + k e = k v λ 1 λ , λ2 λ 2 i=1 i λ i = (λ λ2 2 + w2 i ), λ i = (λ 2 1 λ2 2 + w2 i )2 + 4λ 2 1 λ2 2. ( 1 Since k v + ) n λ i λ 2 1 +λ2 i=1 λ 2 i 0, we see that λ1 has to be negative. This completes the proof. We are now ready to prove Theorem 2. We first consider the orientation part (8c) and (24) with (17). Then, we see from Lemma 3 that the statement of Lemma 1 also holds for (8c) and (24) with u v = k v e e by just replacing S ω in Section 3 by S ω = 1 k v S ω with the function S ω defined in Lemma 3. We thus have U R = u T er e e R(eˆξθ )and U R k e e R (eˆξθ e ) 2 for the input u e = K e e e. Since all the elements of x ω are bounded from its definition, we can define a compact positively invariant set on the space of x ω and LaSalle invariance principle is applicable.

6 SICE JCMSI, Vol. 5, No. 5, September Fig. 7 Target object (omni-directional mobile robot). Fig. 9 Measured and estimated poses. Fig. 8 Experimental schematic. Consequently, the orientation and angular velocity estimation errors converge to zero similarly to Lemma 2. We next consider the estimation error of the position and linear velocity. The combination of the position part of (6b) and (25) is formulated as (15a) and (15b) by just replacing Φ by Γ and x p = (v e, p e )byx p = (x ev, p e ). From Lemma 4, the origin of the nominal system ẋ p = Γx p is exponentially stable. Since the definition of ψ is the same as (15b), the parameters γ(t) andδ(t) are also the same. Now, both of the parameters are vanishing because φ(eˆξθ e ) goes to 0 and v wo is bounded from its definition. The remaining part is proved in the same way as Theorem Experimental Verification This section demonstrates the effectiveness of the presented estimation mechanism through experiments, where we compare the estimation result by the presented observer with the conventional observer [12]. We use a pinhole type USB cameras: Firefly MV (View- PLUS), which is vertically attached to the ceiling so that it looks down a field. As a target object, an omni-directional mobile robot is prepared on the field, where we attach four colored apparent feature points to the robot (Fig. 7) so that the box connecting the points from the camera s view forms a square as long as the target is on the field. Note that the robot is compensated by a local proportional-integral controller so that it follows a velocity reference sent by a PC via a wireless communication device XBee (Digi International). The image information is sent to a PC and processed to extract the feature points by the image processing library OpenCV 2.0. The presented observer is implemented on a digital signal processor from dspace Inc. The experimental schematic is summarized in Fig. 8. In the experiment, we send the following linear and angular velocity reference from the PC to the target. v wo = [ 0.4 sin π/6 0.4 sin π/6 0 ] [m/s], ω wo = [ 0 0 π/3 ] [rad/s]. To estimate the motion correctly, we run the presented visual motion observer integrating the motion model with Fig. 10 Measured and estimated velocities A v = A ω = 0 0 I 3, C v = C ω = [ I 3 I 3 0 ] 0 (π/6)i 3 0 where we set k e = k v = 1. Figure 9 shows the time responses of the object pose measured by the camera (dash-dotted curves), estimated by the conventional observer (dashed curves), and estimated by the presented observer (solid curves). The 3rd element of the measured position and 1st and 2nd elements of the measured orientation are fixed to zero based on the prior knowledge that the target must be on a ground plane. We see from the figure that while the conventional observer causes undesirable offsets for all elements of positions and orientations, the presented observer almost cancels these offsets and estimation accuracy drastically improves. Figure 10 illustrates the responses of body velocities, where the meaning of each line type is the same as Fig. 9 but we omit the responses for the conventional observer since it is clear to be zero. We see from the figure that most of the elements are almost correctly estimated by the presented observer while the first and second elements of the angular velocity include unignorable estimation errors. The reason for the errors can be explained as below. As stated above, the feature points ideally form a square from the camera s view but inaccurate image processing techniques and a distortion of the lens provide a distorted box. This implies that the pose consistent with the data gets out of the plane and the observer estimates the contaminated pose and its velocity. Even if the error is small, the contaminated pose can have a large angular velocity in the first and second elements. Thus, our conclusion on the issue is that

7 282 SICE JCMSI, Vol. 5, No. 5, September 2012 the estimation error should be reduced not by the observer but through the thorough calibrations and improvement of image processing techniques. The results except for the elements show the effectiveness of the presented observer and estimation accuracy is at least far better than the conventional observer. 6. Conclusions This paper has addressed vision-based 3D rigid-body motion estimation assuming that a target motion pattern is available for estimation. In particular, a novel vision-based motion observer integrating target motion models has been proposed. The authors first have considered a constant velocity model and proved that the presented observer drives both of the estimates of the object pose and body velocity to their actual values. Moreover, the result has been extended to a more general motion model. Finally, the effectiveness of the presented estimation mechanism has been demonstrated through experiments. References [1] G. Chesi and K. Hashimoto (Eds): Visual Servoing via Advanced Numerical Methods, Lecture Notes in Control and Information Sciences, Vol. 401, Springer, [2] Y. Ma, S. Soatto, J. Kosecka, and S.S. Sastry: An Invitation to 3-D Vision, Springer, [3] Y. Iwatani, K. Watanabe, and K. Hashimoto: Unmanned helicopter control via visual servoing with occlusion handling, Lecture Notes in Control and Information Sciences, Vol. 401, G. Chesi and K. Hashimoto (Eds), Springer, pp , [4] K. Kogiso, M. Tatsumi, and K. Sugimoto: A remote control technique with compensation for transmission delay and its application to pan control of a network camera, Proc. 17th IEEE International Conference on Control Applications, pp , [5] K. Hirata, Y. Kimura, and K. Sugimoto: Visual feedback control of cart-pendulum systems with Webcam, Proc. 4th IEEE International Conference on Mechatronics, ThM1-B-4, [6] S. Takahashi and B.K. Ghosh: Motion and shape identification with vision and range, IEEE Trans. on Automatic Control, Vol. 47, No. 8, pp , [7] F. Chaumette and S. Hutchinson: Visual servo control, Part I: Basic approaches, IEEE Robotics and Automation Magazine, Vol. 13, No. 4, pp , [8] F. Chaumette and S. Hutchinson: Visual servo control, Part II: Advanced approaches, IEEE Robotics and Automation Magazine, Vol. 14, No. 1, pp , [9] P.A. Vela and I.J. Ndiour: Estimation theory and tracking of deformable objects, Proc IEEE International Symposium on Computer-Aided Control System Design, pp , [10] A.P. Dani: Lyapunov-Based Nonlinear Estimation Methods with Applications to Machine Vision, Doctoral dissertation, Department of Mechanical and Aerospace Engineering, University of Florida, [11] A. Aguiar and J. Hespanha: Robust filtering for deterministic systems with implicit outputs, Systems and Control Letters, Vol. 58, No. 4, pp , [12] M. Fujita, H. Kawai, and M.W. Spong: Passivity-based dynamic visual feedback control for three dimensional target tracking: stability and L2-gain performance analysis, IEEE Trans. Control Systems Technology, Vol. 15, No. 1, pp , [13] F. Bensalah and F. Chaumette: Compensation of abrupt motion changes in target tracking by visual servoing, Proc IEEE/RSJ International Conference on Intelligent Robots Systems, pp , [14] R. Ginhoux, J. Gangloff, M. de Mathelin, L. Soler, M.A. Sanchez, and J. Marescaux: Active filtering of physiological motion in robotized surgery using predictive control, IEEE Trans. on Robotics, Vol. 21, No. 1, pp , [15] H.K. Khalil: Nonlinear Systems, Prentice-Hall, Takeshi HATANAKA (Member) He received the B.Eng. degree in informatics and mathematical science, the M.Inf. and Ph.D. degrees in applied mathematics and physics all from Kyoto University, Japan, in 2002, 2004, and 2007, respectively. From 2006 to 2007, he was a research fellow of the JSPS. He is currently an Assistant Professor in the Department of Mechanical and Control Engineering, Tokyo Institute of Technology, Japan. His research interests include cooperative control, vision based control and estimation, and predictive control. Masayuki FUJITA (Member) He is a Professor with the Department of Mechanical and Control Engineering at Tokyo Institute of Technology. He received the Dr. of Eng. degree in Electrical Engineering from Waseda University, Tokyo, in Prior to his appointment at Tokyo Institute of Technology, he held faculty appointments at Kanazawa University and Japan Advanced Institute of Science and Technology. His research interests include passivity-based visual feedback, cooperative control, and robust/predictive control with its industrial applications. He is currently the CSS Vice President Conference Activities. He serves a Control Division Head of the Society of Instrument and Control Engineers (SICE) and served a Director of SICE. He served as the General Chair of the 2010 IEEE Multi-conference on Systems and Control. He was a member of CSS Board of Governors. He has served/been serving as an Associate Editor for the IEEE Transactions on Automatic Control, the IEEE Transactions on Control Systems Technology, Automatica, Asian Journal of Control, and an Editor for the SICE Journal of Control, Measurement, and System Integration. He is a recipient of the 2008 IEEE Transactions on Control Systems Technology Outstanding Paper Award. He also received the 2010 SICE Education Award and the Outstanding Paper Awards from the SICE.