- Email: [email protected]
IMAGE-BASED TARGET TRACKING WITH MULTI-CONSTRAINTS SATISFACTION
Preprints of the 5th IFAC Symposium on Robust Control Design
ROCOND'06, Toulouse, France, July 5-7, 2006
IMAGE-BASED TARGET TRACKING WITH MULTI-CONSTRAINTS SATISFACTION Bo Gao 1 Sophie Tarbouriech 1 Philippe Sou` eres 1
LAAS-CNRS 7 Avenue du Colonel Roche, 31077 Toulouse cedex 4, France.
Abstract: This paper presents a multicriteria image-based controller that allows to track moving targets with square integrable velocity. The proposed approach is based on both polytopic and norm-bounded representations of uncertainties and uses an original sector condition for the description of saturations. The controller allows to stabilize the camera despite depth uncertainty, bounds on visual features errors to ensure visibility, and limits on the camera velocity and acceleration. We determine a neighborhood of the origin inside which the system trajectories remain during the tracking. Asymptotic stability of the closed-loop system is proved when the target is at rest. Keywords: Visual servoing, constraint satisfaction, L2 -disturbances, saturation control, visibility,
1. INTRODUCTION 2D-visual servoing aims at controlling robotic systems by regulating feature errors directly in the camera image-plane. In order to deal with the uncertainty of certain parameters, and take into account additional constraints such as visibility or actuators limitation, the design of robust control schemes turns out to be essential. As shown in (Malis and Rives, 2003), uncertainty in the depth distribution of target points may strongly reduce the domain of stability of the system. However, in most part of applications of image-based control the depth of the target points is unknown, and the interaction matrix is computed at the final position. An adaptive 2Dvision based servo controller robust with respect to depth measurement was proposed in (Zachi et al., 2004). Ensuring the visibility during the motion is also an important issue. Different tracking methods combining geometric informations with local image measurements have been proposed to this end (Marchand and Chaumette, 2005). The idea of mixing feature-based control with pathplanning in the image was proposed in (Mezouar 1
E-mail addresses: {bgao, tarbour, [email protected]}
and Chaumette, 2002). Robust control is particularly interesting when dealing with moving targets for which the unknown velocity can be bounded in some sense. In (Cuvillon et al., 2005), GPC and H∞ fast visual servoing were proposed for tracking moving target points with a medical manipulator. Advanced control techniques allow to consider different kinds of constraints simultaneously at the control synthesis level. A general framework for image-based positioning control design, based on robust quadratic methods and differential inclusions, was proposed in (Tarbouriech and Sou`eres, 2000). The main drawback of this approach was that the conditions were given in the form of Bilinear Matrix Inequalities. A unified approach for position-based and image-based control, through rational systems representation, was also proposed in (Bellot and Dan`es, 2001). In this paper we present a multicriteria imagebased controller that allows to track moving targets with square integrable velocity. The considered approach is based on both polytopic and norm-bounded representations of uncertainties and uses an original sector condition for the description of saturations. The proposed controller allows to stabilize the camera despite unknown value of the target points depth, bounds on admis-
Preprints of the 5th IFAC Symposium on Robust Control Design
sible visual features errors to ensure visibility, and limits on the camera velocity and acceleration. We use Lyapunov analysis and LMI-based optimization procedures to characterize a compact neighborhood of the origin inside which the system trajectories remain during the tracking. When the target is at rest, the asymptotic stability of the closed-loop system is proved and a maximal stability domain is determined. Notations. For any x ∈ B means that A − B is positive definite. A0 denotes the transpose of A. D(x) denotes a diagonal matrix obtained from vector x. 1m denotes the m-order vector 1m = [1 . . . 1]0 ∈ 0, i = 1, ..., m.
2. PROBLEM STATEMENT We consider that the camera, which is supported by a robotic system, is free to execute any horizontal translations and rotations about the vertical axis. The target is made of three points Ei , i = 1, 2, 3, equispaced on a horizontal line, and located at the the same height as the camera optical center C. Let R be a frame attached to the scene and Rc a frame attached to the camera, having its origin at C and its z-axis directed along the optical axis. Let T ∈ <3 denote the reduced kinematic screw of the camera which expresses the translational and rotational velocities of RC with respect to R. The target is assumed to move as if it was fixed to a unicycle: it can rotate about its central point E2 whereas its linear velocity vE is always perpendicular to the line (E1 , E3 ) as indicated in Fig. 1. ωE denotes the target’s rotational velocity.
ROCOND'06, Toulouse, France, July 5-7, 2006
In return, we make the hypothesis that the camera intrinsic parameters are known and consider the metric pinhole model with focal length f = 1. In the sequel, we will give a description of the controller for a general robotics system. An application to the case of a camera mounted on a wheeled robot will be considered in section 4. We will denote by l > 0 the distance between the target points, α the angle between the target line and the optical axis, and η the angle between the optical axis and the line (CE2 ) (see figure 1). Without lost of generality we will assume that the camera is initially located in the left halfplane delimited by the line (E1 , E3 ) and that the distance d = CE2 is bounded as follows: d ∈ [dmin , dmax ]
Furthermore, to prevent from projection singularities, the following condition is considered: α ∈ [−π + αmin , −αmin ]
C1: The depth of the target points with respect to the camera frame, is bounded but unknown. C2: The visual signal errors, in the image, must remain bounded during the stabilization process to ensure visibility. C3: The velocity and the acceleration of the camera remain bounded to satisfy the limits on the actuators dynamics. C4: The velocity vector of the target is supposed to be square integrable but unknown.
(2)
where αmin > 0 is a small angle. For i = 1, 2, 3, let us denote respectively by Yi and Yi∗ the ordinates of the current and desired target points in the image. Following the formalism introduced in (Espiau et al., 1992), a simple choice for the 0 positioning task function is: e = [ e1 e2 e3 ] ∈ <3 ∗ with ei = Yi − Yi . The desired camera position corresponds to: Y2∗ = 0 and Y3∗ = −Y1∗ . In addition, to guarantee the visibility of the target, the condition C2 is specified by imposing the following bound on task components: C2 :
|ei | ≤ β, i = 1, 2, 3 ; with β > 0
(3)
Consequently, the angle η is bounded by: |η| ≤ ηmax = arctan (β) < π/2
(4)
and therefore, the depth z2 = d cos(η) of the central point E2 is bounded as follows: z2 ∈ [dmin cos(ηmax ), dmax ]
(5)
The relation between the time-derivative of the task function and the kinematic screw is given by the optical flow equations: e˙ = L(z, e)T +
Fig. 1. Description of the vision-based task The objective of the paper is to design a controller to stabilize the camera with respect to the target despite the following conditions:
(1)
∂e ∂t
(6)
where L(z, e) ∈ <3×3 is the image Jacobian: −1/z1 (e1 + Y1? )/z1 1 + (e1 + Y1? )2 L(z, e) = −1/z2 (e2 + Y2? )/z2 1 + (e2 + Y2? )2 −1/z3 (e3 + Y3? )/z3 1 + (e3 + Y3? )2 (7) The term ∂e , which represents task variation ∂t due to the target motion, can be expressed by the relation ∂e ∂t = B(z, e)ω(t) where the matrix B(z, e) ∈ <3×4 is defined by: " # ? ? B(z, e) =
1/z1 −(e1 + Y1 )/z1 1/z1 −(e1 + Y1 )/z1 1/z2 −(e2 + Y2? )/z2 0 0 1/z3 −(e3 + Y3? )/z3 −1/z3 (e3 + Y3? )/z3 (8) −vE cos(α) −vE sin(α) and ω(t) = −lω ∈ <4 E cos(α) −lωE sin(α)
Preprints of the 5th IFAC Symposium on Robust Control Design
ROCOND'06, Toulouse, France, July 5-7, 2006
0
As the target velocity vector [ vE ωE ] is supposed to be square integrable but unknown, the vector ω(t) will be considered as a disturbance vector. With this notation, C4 can be specified as follows:
which means that the state ξ must belong to the following polyhedral set Ω(ξ): β1 β1 Ω(ξ) = {ξ ∈ <6 ; − u 3 ξ u 3 } (18)
C4 : ω(t) ∈ Lq2 and ∃δ1 > 0, such that, ∀t > 0: Z t Z t
The problem we intend to solve with respect to the closed-loop system (17), subject to constraints (18), can then be summarized as follows.
ω(τ )0 ω(τ )dτ =
kω(t)k22 =
0
0
1 2 2 (τ ))dτ ≤ (τ )+l2 ωE (vE δ1 (9)
In (6) and (7), the constraints on the vector 0 z = [ z1 z2 z3 ] ∈ <3 follow the description given in Fig.1. From relations (1), (2), (4) and (5), the depth of target points E1 and E3 can be expressed in terms of the depth z2 of the central point E2 : 0
z = [z2 + l cos(α), z2 , z2 − l cos(α)]
As l << z2 the following approximation can be done: 1 1 l cos(α) ' (1 − )= z1 z2 z2 1 = p1 z2 1 1 l cos(α) ' (1 + )= z3 z2 z2
1 l cos(α) − = p1 − p2 z2 z22 (10) 1 l cos(α) + = p1 + p2 z2 z22
From the definition of the admissible intervals relative to z2 and α given by (2) and (5), it follows that the scalars p1 and p2 in (10) satisfy pjmin ≤ pj ≤ pjmax , j = 1, 2. Thus, the matrices L(z, e) and B(z, e) depend on two uncertain parameters p1 and p2 . To take into account the limits on the actuators dynamics, we precise the statement of condition C3 by introducing the following bounds on the camera velocity and acceleration:
1
1
Problem 1. Determine a gain K and two regions S0 and S1 , as large as possible, such that in spite of conditions C1, and C4, the constraints C2 and C3 be satisfied and: • when ω = 0, for any ξ(0) ∈ S0 the closed-loop trajectories of system (17) converge asymptotically to the origin. • when ω 6= 0, the closed-loop trajectories remain bounded in S1 for any ξ(0) ∈ S1 and for all ω(t) satisfying (9). 3. CONTROL SYNTHESIS The closed-loop system (17) can be described by: ξ˙ = (A(z, ξ) + B1 K)ξ + B1 φ(Kξ) + B2 (z, ξ)ω(t) (19) with the decentralized dead-zone nonlinearity φ(Kξ) = satu0 (Kξ) − Kξ, defined by: ∀i = 1, 2, 3, ( φ(K(i) ξ) =
u0(i) − K(i) ξ if K(i) ξ > u0(i) 0 if |K(i) ξ| ≤ u0(i) −u0(i) − K(i) ξ if K(i) ξ < −u0(i)
Consider a matrix G ∈ <3×6 and define the following set: S(u0 ) = {ξ ∈ <6 ; −u0 (K − G)ξ u0 }
−u1 T u1 −u0 T˙ u0
(11) (12)
To model the control problem the following extended state vector is defined: e ξ = T ∈ <6 (13) with the following matrices 0 L(z, e) A(z, ξ) = 0 ∈ <6×6 0 0 B(z, e) B1 = I ∈ <6×3 ; B2 (z, ξ) = ∈ <6×4 0 3
(14) Thus, we consider the following system ξ˙ = A(z, ξ)ξ + B1 T˙ + B2 (z, ξ)ω(t)
(15)
where the acceleration of the camera T˙ is the control vector. To cope with constraint (12), the control law we consider has the following form: T˙ = satu0 (Kξ) with K = [ K1 K2 ] ∈ <3×6 (16) Hence, the closed-loop system reads: ξ˙ = A(z, ξ)ξ + B1 satu0 (Kξ) + B2 (z, ξ)ω(t) (17) Relative to the closed-loop system (17), one has to take into account the constraints (3) and (11),
(20)
(21)
The following lemma can then be stated. Lemma 1. (Gomes da Silva Jr J.M. and Tarbouriech, 2005) Consider the nonlinearity φ(Kξ) defined in (20). If ξ ∈ S(u0 ) then the relation: φ(Kξ)0 M (φ(Kξ) + Gξ) ≤ 0
(22)
is satisfied for any diagonal positive definite matrix M ∈ <3×3 . Let us now consider a positive definite function V (ξ) > 0, ∀ξ 6= 0, with V (0) = 0, such that its time-derivative along the trajectories of the closed-loop saturating system (19) verifies V˙ (ξ) ≤ ω(t)0 ω(t) (23) for all ω(t) satisfying (9) and ∀ξ ∈ D, where D is some bounded domain inside the basin of attraction of (19). If (23) is verified, it follows that Z t V (ξ(t)) − V (ξ(0)) ≤ ω(τ )0 ω(τ )dτ (24) 0
∀ξ ∈ D and ∀t ≥ 0. From (9), we have V (ξ(t)) ≤ V (ξ(0)) + δ11 and, hence, V (ξ(0)) ≤
1 1 1 =⇒ V (ξ(t)) ≤ + ζ ζ δ1
(25)
Define the sets S1 = {ξ ∈ <6 ; V (ξ) ≤ ζ1 + δ11 } and S0 = {ξ ∈ <6 ; V (ξ) ≤ ζ1 }. Provided that
Preprints of the 5th IFAC Symposium on Robust Control Design
ROCOND'06, Toulouse, France, July 5-7, 2006
S1 is included in the basin of attraction of (19), from (25) it follows that the closed-loop system trajectories remain bounded in S1 , for any initial condition ξ(0) ∈ S0 , and any ω(t) satisfying (9). Introducing the matrices R = [ I3 0 ] and C = [ 0 I3 ], we consider the following theorem. Theorem 1. If there exist a positive definite function V (ξ) (V (ξ) > 0, ∀ξ 6= 0), a gain K, a positive definite diagonal matrix M , a matrix G and two positive scalars ζ and δ1 satisfying, for any admissible z and i = 1, 2, 3: ∂V [(A(z, ξ) + BK)ξ + Bφ(Kξ)] ∂ξ −2φ(Kξ)0 M (φ(Kξ) + Gξ) − ω 0 ω < 0 V (ξ) − ξ 0 (K(i) − G(i) )0
1 ζ
V (ξ) − ξ
1 δ1
u20(i)
V (ξ) − ξ 0 R0(i) 0
+
1 ζ
+ β2
1 1 ζ + δ C0(i) 2 1 u1(i)
R(i) ξ ≥ 0 C(i) ξ ≥ 0
(27)
(28) (29)
then the gain K and the sets S1 (V, ζ, δ1 ) = {ξ ∈ <6 ; V (ξ) ≤ ζ1 + δ11 } and S0 (V, ζ) = {ξ ∈ <6 ; V (ξ) ≤ ζ −1 } are solutions to Problem 1. Proof. The satisfaction of (27) means that the set S1 (V, ζ, δ1 ) is included in the set S(u0 ) defined as in (21). Thus, one can conclude that for any ξ ∈ S1 (V, ζ, δ1 ) the nonlinearity φ(Kξ) satisfies the sector condition (22). Moreover, the satisfaction of relations (28) and (29) means that the set S1 (V, ζ, δ1 ) is included in the set Ω(ξ) defined by (18). Thus, for any ξ ∈ S1 (V, ζ, δ1 ), the constraints C2 and C3 are respected. Consider a positive definite function V (ξ) (V (ξ) > 0, ∀ξ 6= 0). We want to prove that the time-derivative of this function satisfies (23) along the trajectories of the closedloop system (19) for all admissible nonlinearity φ(Kξ), all admissible uncertain vector z and all disturbances satisfying (9). Hence, using Lemma 1, one gets: V˙ (ξ) − ω 0 ω ≤ V˙ (ξ) − 2φ(Kξ)0 (φ(Kξ) + Gξ) − ω 0 ω
B2 (z) =
"
1/z1 0 0 0 1/z2 0 0 0 1/z3
B4 (z) =
B5 (z) =
"
"
#
; B3 =
"
;
2Y1∗ 0 0 0 2Y2∗ 0 0 0 2Y3∗
1/z1 −Y1∗ /z1 1/z1 −Y1∗ /z1 1/z2 −Y2∗ /z2 0 0 1/z3 −Y3∗ /z3 −1/z3 Y3∗ /z3
0 −1/z1 0 −1/z1 0 −1/z2 0 0 0 −1/z3 0 1/z3
#
; D(e) =
"
#
#
;
;
#
e1 0 0 0 e2 0 0 0 e3
the closed-loop system reads: ξ˙ = (R0 B1 (z)C + R0 T(2) B2 (z)R + B1 K +R0 T(3) (B3 + D(e))R)ξ + B1 φ(Kξ) +R(B4 (z) + D(e)B5 (z))ω(t)
.
(30)
Thus, from (10), B1 (z), B2 (z), B4 (z) and B5 (z) depend on two uncertain parameters p1 and p2 . It follows that these matrices belong to a polytope with 4 vertices given by the combinations of value of p1 and p2 in their definition interval: Bk (z) ∈ Co{Bkj , j = 1, ..., 4}, for k = 1, 2, 4, 5. (31) The following proposition derived from Theorem 1 can then be stated. Proposition 1. If there exist symmetric positive definite matrices W ∈ <6×6 , R1 ∈ <3×3 , a diagonal positive matrix S ∈ <3×3 , two matrices Y ∈ <3×6 and Z ∈ <3×6 , three positive scalars , ζ and δ1 satisfying 2 : W A0 + A W + B Y 1j
0 0
1j
0
1
+Y B1 + R R1 R +(u21(3) (1 + β 2 ) + β 2 )R0 R uh1(2) Bi2j RW B3 RW I3 0 SB − Z 0 B4j R 0
?
?
?
?
?
−R1
?
?
?
?
0 −I6 ?
?
?
0 0 0
<0
0 −2S ? ? 0 0 −I4 ? 0 0 B5j −I3
(32)
Thus, the satisfaction of relation (26) implies that
"
V˙ (ξ) − 2φ(Kξ)0 M (φ(Kξ) + Gξ) − ω 0 ω < 0 along the trajectories of system (19). Relations (24) and (25) are then satisfied. Therefore, when w = 0, one gets V˙ (ξ) < 0, ∀ξ(0) ∈ S1 (V, ζ, δ1 ). When w 6= 0, the closed-loop trajectories of system (19) remain bounded in S1 (V, ζ, δ1 ), for any ξ(0) ∈ S0 (V, ζ) and any disturbance satisfying (9). One can conclude that the gain K and the sets S1 (V, ζ, δ1 ) = {ξ ∈ <6 ; V (ξ) ≤ ζ1 + δ11 } and S0 (V, ζ) = {ξ ∈ <6 ; V (ξ) ≤ ζ −1 } are solutions to Problem 1. 2 Theorem 1 provides a sufficient condition to solve the control gain design. However, such a condition appears not really constructive to exhibit a suitable function V (ξ), a gain K and two scalars ζ and δ1 . The idea then consists in considering a quadratic function for V (ξ), as V (ξ) = ξ 0 P ξ,
−1/z1 Y1∗ /z1 1 + (Y1∗ )2 −1/z2 Y2∗ /z2 1 + (Y2∗ )2 −1/z3 Y3∗ /z3 1 + (Y3∗ )2
B1 (z) =
(26)
(K(i) − G(i) )ξ ≥ 0 1 δ1
P = P 0 > 0. Furthermore, we have also to write in a tractable way the matrices A(z, ξ) and B2 (z, ξ), and therefore the closed-loop system (19). In this sense, we define:" #
W ? ? Y(i) − Z(i) ζu20(i) ? Y(i) − Z(i) 0 δ1 u20(i)
" "
W ? ? R(i) W ζβ 2 ? R(i) W 0 δ1 β 2
#
W ? ? C(i) W ζu21(i) ? C(i) W 0 δ1 u21(i)
#
≥0
(33)
≥0
(34)
#
(35)
≥0
∀i = 1, 2, 3, then, the control gain K ∈ <3×6 given by K = Y W −1 is such that: (i) when ω 6= 0, the trajectories of the closed-loop system (17) remain bounded in the set o n E1 (W, ζ, δ1 ) =
2
ξ ∈ <6 ; ξ 0 W −1 ξ ≤
? stands for symmetric blocks.
1 1 + ζ δ1
(36)
Preprints of the 5th IFAC Symposium on Robust Control Design
for any ξ(0) ∈ E0 (W, ζ), n E0 (W, ζ) =
ξ ∈ <6 ; ξ 0 W −1 ξ ≤
1 ζ
ROCOND'06, Toulouse, France, July 5-7, 2006
o
(37)
and for any ω(t) satisfying (9); (ii) when ω = 0, the set E0 (W, ζ) = E1 (W, ζ, δ1 ) is included in the basin of attraction of the closedloop system (17) and is contractive. Proof. The proof mimics the one of Theorem 1. The satisfaction of relation (33) means that the set E1 (W, ζ, δ1 ) defined in (36) is included in the set S(u0 ) defined in (21). Thus, one can conclude that for any ξ ∈ E1 (W, ζ, δ1 ) the nonlinearity φ(Kξ) satisfies the sector condition (22) with G = ZW −1 . Furthermore, the satisfaction of relations (34) and (35) implies that the set E1 (W, ζ, δ1 ) is included in the sets Ω(ξ) defined in (18). Hence, for any admissible uncertain vector z (see equation (31)) and any admissible vector belonging to E1 (W, ζ, δ1 ), the closed-loop system (19) or (30) can be written as: ξ˙ =
4 X
j=1 (38) +R0 T(3) (B3 + D(e))R + B1 K]ξ +R0 (B4j + D(e)B5j )ω} + B1 φ(Kξ) 4 X with λj = 1, λj ≥ 0 , and A1j = R0 B1j C. j=1
The time-derivative of V (ξ) = ξ 0 W −1 ξ along the trajectories of system (38) writes: 4 X
λj {[A1j + R0 T(2) B2j R
j=1
+R0 T(3) (B3 + D(e))R + B1 Y W −1 ]ξ +R0 [B4j + D(e)B5j ]ω} + 2ξ 0 W −1 B1 φ(Kξ) −2φ(Kξ)0 M (φ(Kξ) + ZW −1 ξ) − ω 0 ω By convexity one can prove that the right term of the above inequality is negative definite if the following condition is verified: 0
2ξ W
−1
0
Optimization Issues It is important to note that relations (32), (33), (34) and (35) of Proposition 2 are LMIs. Depending on the energy bound on the disturbance, δ1 , is given by the designer or not, the following optimization problems can be considered: • given δ1 , we want to optimize the size of the sets E0 and E1 . This case can be addressed by the following convex optimization problem:
λj {[A1j + R0 T(2) B2j R
V˙ (ξ) − ω 0 ω ≤ 2ξ 0 W −1
Thus, the satisfaction of relations (32) to (35) with M = S −1 , K = Y W −1 and G = ZW −1 allows to verify that V˙ (ξ) < ω 0 ω (39) for all ξ(0) ∈ E0 and any ω(t) satisfying (9). Hence the trajectories of the closed-loop system (17) remain bounded in E1 (W, ζ, δ1 ) for all ξ(0) ∈ E0 (W, ζ) and any ω(t) satisfying (9). This completes the proof of statement (i). To prove statement (ii), assume that ω = 0. Then, from (39), we have V˙ (ξ) < 0, ∀ξ ∈ E1 , which means that E0 = E1 is a set of asymptotic stability for system (19). 2
0
[A1j + R T(2) B2j R + R (B3 + D(e))R +B1 Y W −1 ]ξ + 2ξ 0 W −1 B1 φ(Kξ) +2ξ 0 W −1 R0 (B4j + D(e)B5j )ω −2φ(Kξ)0 M (φ(Kξ) + ZW −1 ξ) − ω 0 ω < 0
min
W,R1 ,Y,Z,S,,ζ
ζ +δ+σ
subject to relations (32), (33), (34),(35), δI6 ? σI6 ? Y I3 ≥ 0, I6 W ≥ 0
(40)
The last two constraints are added to guarantee a satisfactory conditioning number for K and W . • δ1 being a decision variable, we want to minimize it. This problem comes to find the largest disturbance tolerance. In this case, we can add δ1 in the previous criteria; the other decision variables may be kept in order to satisfy a certain trade-off between the size of the sets E0 , E1 and δ1 . 4. APPLICATION In this section we present a result of simulation of the proposed controller in the case that the camera is mounted on a pan-platform supported by a wheeled robot (Fig. 2). x and y are the coordi-
Thus, by using (11) one can upper-bound the term containing T(2) as follows 2ξ 0 W −1 R0 T(2) B2j Rξ ≤ ξ 0 (W −1 R0 R1 RW −1 0 +u21(2) R0 B2j R1−1 B2j R)ξ with R1 = R10 > 0. In the same way, by using both (11) and (18), one can upper-bound the term containing T(3) and D(e) as follows: 2ξ 0 W −1 R0 (T(3) (B3 + D(e))Rξ +"D(e)B5j ω)#= B3 R 0 h ξ i R 0 2ξ 0 W −1 R0 T(3) I3 T(3) D(e) D(e) ω 0 B5j 2 2 2 0 −1 0 −1 ≤ (u1(3) (1 + β ) + β )ξ W R RW ξ
−1
+
ξ0 ω
0
"
B3 R 0 R 0 0 B5j
#0 "
B3 R 0 R 0 0 B5j
#
h i ξ ω
Fig. 2. Camera fixed on a pan-platform supported by a wheeled robot nates of the robot reference point M with respect to R(O, X, Y, Z), θ is the direction of the vehicle with respect to the X-axis, and θp is the direction of the pan-platform with respect to the robot’s main direction. A frame RP (P, XP , YP , ZP ) is attached to the pan-platform whose origin is at the center of rotation P . The transformation between RP and RC consists of an horizontal translation
Preprints of the 5th IFAC Symposium on Robust Control Design
ROCOND'06, Toulouse, France, July 5-7, 2006
0
of vector [ a b 0 ] and a rotation of angle π2 about the YP -axis. Dx is the distance between M and P . The velocities of the robot, which constitutes the actual system inputs are described by the vector q˙ = [v1 v2 v3 ]0 , where v1 and v2 are the linear and the angular velocities of the cart with respect to R, while v3 is the pan-platform angular velocity with respect to RM . We consider the following kinematic model: x˙ cos(θ) 0 0 " # v y˙ sin(θ) 0 0 1 θ˙ = 0 1 0 v2 v3 0 01 θ˙p
Fig. 3. Trajectory of the robot (left) and visual features evolution (right)
The kinematic screw T is linked to the velocity vector by the relation T = J(q)q, ˙ in which the robot Jacobian is given by: " # − sin(θp ) Dx cos(θp ) + a a J(q)= cos(θp ) Dx sin(θp ) − b −b (41) 0 −1 −1 As shown in (Pissard-Gibollet and Rives, 1995), the rotational degree of freedom of the pan platform allows to overcome the nonholonomic constraint. Using the proposed control method, the robot tracks correctly the target and get stabilized at the expected position, despite uncertainties on the target point depth. With respect to the absolute frame R, the coordinates of the target points are: E1 (10m, 0.5m), E2 (10m, 0m) and E3 (10m, −0.5m). The interval of distance between the robot and camera is d ∈ [2.454m, 8m]. At the expected position, the visual features are defined by: Y1∗ = 0.2, Y2∗ = 0, and Y3∗ = −0.2. To guarantee the visibility we considered β = 0.4. The initial robot’s configuration is given by: x = 4.85m, y = −0.8m, θ0 = 0rad and θp = 0.175rad, and the initial value of the state vector is: ξ(0) = [−0.12m; −0.0159m; 0.0849m; 0ms−1 ; 0ms−1 ; 0 rad−1 ]. The bounds on the camera velocity and acceleration are u1 = [1 1 0.1]0 , and u0 = [1 1 5]0 . By applying the proposed control scheme with the Matlab LMI Control Toolbox we obtained the following value for the control gain K: " # K=
−505 1392.8 −507.8 −6.5 0.1 8.6 −814 35.1 784.8 0.1 −12.5 0.1 −937.5 −161.2 −973.5 8.6 0.1 −72.8
This value of K was obtained for the scalar δ1 = 12.1949. Fig. 3 represents the robot trajectory and the evolution of the visual features. The velocities of the robot and the kinematic screw of the camera are described in Fig. 4. One can see in Fig. 5 that the second components saturate during the very beginning of the task. REFERENCES Bellot, D. and P. Dan`es (2001). Handling visual servoing schemes through rational systems and lmis. IEEE Int. Conference on Decision and Control 4, 3601–3606. Cuvillon, L., E. Laroche, J. Gangloff and M. de Mathelin (2005). GPC versus H∞ control for fast visual servoing of medical manipulator including flexibilities. IEEE Int. Conference on Robotics and Automation.
Fig. 4. Robot’ velocities (left), and camera’s kinematic screw T (right)
Fig. 5. Description of the control components during the very beginning of the task) Espiau, B., F. Chaumette and P. Rives (1992). A new approach to visual servoing in robotics. IEEE Trans. on Robotics and Automation. Gomes da Silva Jr J.M. and S. Tarbouriech (2005). Anti-windup design with guaranteed regions of stability: an lmi-based approach. IEEE Trans. Autom. Control 50(1), 106–111. Malis, E. and P. Rives (2003). Robustness of image-based visual servoing with respect to depth distribution errors. In: IEEE Int. Conf. on Robotics and Automation. Taipei Taiwan. Marchand, E. and F. Chaumette (2005). Feature tracking for visual servoing purposes. Robotics and Autonomous Systems. Mezouar, Y. and F. Chaumette (2002). Path planning for robust image-based control. IEEE Trans. On Robotics and Automation 18(4), 534–549. Pissard-Gibollet, R. and P. Rives (1995). Applying visual servoing techniques to control a mobile hand-eye system. In: IEEE Int Conf. on Robotics and Automation. Nagoya, Japan. Tarbouriech, S. and P. Sou`eres (2000). Advanced control strategies for the visual servoing scheme. IFAC Int. Symp. on Robot Control. Zachi, A.R.L., L. Hsu and F. Lizarralde (2004). Performing stable 2d adaptive visual positionning/tracking control without explicit depth measurement. IEEE Int. Conf. on Robotics and Automation.
ROCOND'06, Toulouse, France, July 5-7, 2006
IMAGE-BASED TARGET TRACKING WITH MULTI-CONSTRAINTS SATISFACTION Bo Gao 1 Sophie Tarbouriech 1 Philippe Sou` eres 1
LAAS-CNRS 7 Avenue du Colonel Roche, 31077 Toulouse cedex 4, France.
Abstract: This paper presents a multicriteria image-based controller that allows to track moving targets with square integrable velocity. The proposed approach is based on both polytopic and norm-bounded representations of uncertainties and uses an original sector condition for the description of saturations. The controller allows to stabilize the camera despite depth uncertainty, bounds on visual features errors to ensure visibility, and limits on the camera velocity and acceleration. We determine a neighborhood of the origin inside which the system trajectories remain during the tracking. Asymptotic stability of the closed-loop system is proved when the target is at rest. Keywords: Visual servoing, constraint satisfaction, L2 -disturbances, saturation control, visibility,
1. INTRODUCTION 2D-visual servoing aims at controlling robotic systems by regulating feature errors directly in the camera image-plane. In order to deal with the uncertainty of certain parameters, and take into account additional constraints such as visibility or actuators limitation, the design of robust control schemes turns out to be essential. As shown in (Malis and Rives, 2003), uncertainty in the depth distribution of target points may strongly reduce the domain of stability of the system. However, in most part of applications of image-based control the depth of the target points is unknown, and the interaction matrix is computed at the final position. An adaptive 2Dvision based servo controller robust with respect to depth measurement was proposed in (Zachi et al., 2004). Ensuring the visibility during the motion is also an important issue. Different tracking methods combining geometric informations with local image measurements have been proposed to this end (Marchand and Chaumette, 2005). The idea of mixing feature-based control with pathplanning in the image was proposed in (Mezouar 1
E-mail addresses: {bgao, tarbour, [email protected]}
and Chaumette, 2002). Robust control is particularly interesting when dealing with moving targets for which the unknown velocity can be bounded in some sense. In (Cuvillon et al., 2005), GPC and H∞ fast visual servoing were proposed for tracking moving target points with a medical manipulator. Advanced control techniques allow to consider different kinds of constraints simultaneously at the control synthesis level. A general framework for image-based positioning control design, based on robust quadratic methods and differential inclusions, was proposed in (Tarbouriech and Sou`eres, 2000). The main drawback of this approach was that the conditions were given in the form of Bilinear Matrix Inequalities. A unified approach for position-based and image-based control, through rational systems representation, was also proposed in (Bellot and Dan`es, 2001). In this paper we present a multicriteria imagebased controller that allows to track moving targets with square integrable velocity. The considered approach is based on both polytopic and norm-bounded representations of uncertainties and uses an original sector condition for the description of saturations. The proposed controller allows to stabilize the camera despite unknown value of the target points depth, bounds on admis-
Preprints of the 5th IFAC Symposium on Robust Control Design
sible visual features errors to ensure visibility, and limits on the camera velocity and acceleration. We use Lyapunov analysis and LMI-based optimization procedures to characterize a compact neighborhood of the origin inside which the system trajectories remain during the tracking. When the target is at rest, the asymptotic stability of the closed-loop system is proved and a maximal stability domain is determined. Notations. For any x ∈
2. PROBLEM STATEMENT We consider that the camera, which is supported by a robotic system, is free to execute any horizontal translations and rotations about the vertical axis. The target is made of three points Ei , i = 1, 2, 3, equispaced on a horizontal line, and located at the the same height as the camera optical center C. Let R be a frame attached to the scene and Rc a frame attached to the camera, having its origin at C and its z-axis directed along the optical axis. Let T ∈ <3 denote the reduced kinematic screw of the camera which expresses the translational and rotational velocities of RC with respect to R. The target is assumed to move as if it was fixed to a unicycle: it can rotate about its central point E2 whereas its linear velocity vE is always perpendicular to the line (E1 , E3 ) as indicated in Fig. 1. ωE denotes the target’s rotational velocity.
ROCOND'06, Toulouse, France, July 5-7, 2006
In return, we make the hypothesis that the camera intrinsic parameters are known and consider the metric pinhole model with focal length f = 1. In the sequel, we will give a description of the controller for a general robotics system. An application to the case of a camera mounted on a wheeled robot will be considered in section 4. We will denote by l > 0 the distance between the target points, α the angle between the target line and the optical axis, and η the angle between the optical axis and the line (CE2 ) (see figure 1). Without lost of generality we will assume that the camera is initially located in the left halfplane delimited by the line (E1 , E3 ) and that the distance d = CE2 is bounded as follows: d ∈ [dmin , dmax ]
Furthermore, to prevent from projection singularities, the following condition is considered: α ∈ [−π + αmin , −αmin ]
C1: The depth of the target points with respect to the camera frame, is bounded but unknown. C2: The visual signal errors, in the image, must remain bounded during the stabilization process to ensure visibility. C3: The velocity and the acceleration of the camera remain bounded to satisfy the limits on the actuators dynamics. C4: The velocity vector of the target is supposed to be square integrable but unknown.
(2)
where αmin > 0 is a small angle. For i = 1, 2, 3, let us denote respectively by Yi and Yi∗ the ordinates of the current and desired target points in the image. Following the formalism introduced in (Espiau et al., 1992), a simple choice for the 0 positioning task function is: e = [ e1 e2 e3 ] ∈ <3 ∗ with ei = Yi − Yi . The desired camera position corresponds to: Y2∗ = 0 and Y3∗ = −Y1∗ . In addition, to guarantee the visibility of the target, the condition C2 is specified by imposing the following bound on task components: C2 :
|ei | ≤ β, i = 1, 2, 3 ; with β > 0
(3)
Consequently, the angle η is bounded by: |η| ≤ ηmax = arctan (β) < π/2
(4)
and therefore, the depth z2 = d cos(η) of the central point E2 is bounded as follows: z2 ∈ [dmin cos(ηmax ), dmax ]
(5)
The relation between the time-derivative of the task function and the kinematic screw is given by the optical flow equations: e˙ = L(z, e)T +
Fig. 1. Description of the vision-based task The objective of the paper is to design a controller to stabilize the camera with respect to the target despite the following conditions:
(1)
∂e ∂t
(6)
where L(z, e) ∈ <3×3 is the image Jacobian: −1/z1 (e1 + Y1? )/z1 1 + (e1 + Y1? )2 L(z, e) = −1/z2 (e2 + Y2? )/z2 1 + (e2 + Y2? )2 −1/z3 (e3 + Y3? )/z3 1 + (e3 + Y3? )2 (7) The term ∂e , which represents task variation ∂t due to the target motion, can be expressed by the relation ∂e ∂t = B(z, e)ω(t) where the matrix B(z, e) ∈ <3×4 is defined by: " # ? ? B(z, e) =
1/z1 −(e1 + Y1 )/z1 1/z1 −(e1 + Y1 )/z1 1/z2 −(e2 + Y2? )/z2 0 0 1/z3 −(e3 + Y3? )/z3 −1/z3 (e3 + Y3? )/z3 (8) −vE cos(α) −vE sin(α) and ω(t) = −lω ∈ <4 E cos(α) −lωE sin(α)
Preprints of the 5th IFAC Symposium on Robust Control Design
ROCOND'06, Toulouse, France, July 5-7, 2006
0
As the target velocity vector [ vE ωE ] is supposed to be square integrable but unknown, the vector ω(t) will be considered as a disturbance vector. With this notation, C4 can be specified as follows:
which means that the state ξ must belong to the following polyhedral set Ω(ξ): β1 β1 Ω(ξ) = {ξ ∈ <6 ; − u 3 ξ u 3 } (18)
C4 : ω(t) ∈ Lq2 and ∃δ1 > 0, such that, ∀t > 0: Z t Z t
The problem we intend to solve with respect to the closed-loop system (17), subject to constraints (18), can then be summarized as follows.
ω(τ )0 ω(τ )dτ =
kω(t)k22 =
0
0
1 2 2 (τ ))dτ ≤ (τ )+l2 ωE (vE δ1 (9)
In (6) and (7), the constraints on the vector 0 z = [ z1 z2 z3 ] ∈ <3 follow the description given in Fig.1. From relations (1), (2), (4) and (5), the depth of target points E1 and E3 can be expressed in terms of the depth z2 of the central point E2 : 0
z = [z2 + l cos(α), z2 , z2 − l cos(α)]
As l << z2 the following approximation can be done: 1 1 l cos(α) ' (1 − )= z1 z2 z2 1 = p1 z2 1 1 l cos(α) ' (1 + )= z3 z2 z2
1 l cos(α) − = p1 − p2 z2 z22 (10) 1 l cos(α) + = p1 + p2 z2 z22
From the definition of the admissible intervals relative to z2 and α given by (2) and (5), it follows that the scalars p1 and p2 in (10) satisfy pjmin ≤ pj ≤ pjmax , j = 1, 2. Thus, the matrices L(z, e) and B(z, e) depend on two uncertain parameters p1 and p2 . To take into account the limits on the actuators dynamics, we precise the statement of condition C3 by introducing the following bounds on the camera velocity and acceleration:
1
1
Problem 1. Determine a gain K and two regions S0 and S1 , as large as possible, such that in spite of conditions C1, and C4, the constraints C2 and C3 be satisfied and: • when ω = 0, for any ξ(0) ∈ S0 the closed-loop trajectories of system (17) converge asymptotically to the origin. • when ω 6= 0, the closed-loop trajectories remain bounded in S1 for any ξ(0) ∈ S1 and for all ω(t) satisfying (9). 3. CONTROL SYNTHESIS The closed-loop system (17) can be described by: ξ˙ = (A(z, ξ) + B1 K)ξ + B1 φ(Kξ) + B2 (z, ξ)ω(t) (19) with the decentralized dead-zone nonlinearity φ(Kξ) = satu0 (Kξ) − Kξ, defined by: ∀i = 1, 2, 3, ( φ(K(i) ξ) =
u0(i) − K(i) ξ if K(i) ξ > u0(i) 0 if |K(i) ξ| ≤ u0(i) −u0(i) − K(i) ξ if K(i) ξ < −u0(i)
Consider a matrix G ∈ <3×6 and define the following set: S(u0 ) = {ξ ∈ <6 ; −u0 (K − G)ξ u0 }
−u1 T u1 −u0 T˙ u0
(11) (12)
To model the control problem the following extended state vector is defined: e ξ = T ∈ <6 (13) with the following matrices 0 L(z, e) A(z, ξ) = 0 ∈ <6×6 0 0 B(z, e) B1 = I ∈ <6×3 ; B2 (z, ξ) = ∈ <6×4 0 3
(14) Thus, we consider the following system ξ˙ = A(z, ξ)ξ + B1 T˙ + B2 (z, ξ)ω(t)
(15)
where the acceleration of the camera T˙ is the control vector. To cope with constraint (12), the control law we consider has the following form: T˙ = satu0 (Kξ) with K = [ K1 K2 ] ∈ <3×6 (16) Hence, the closed-loop system reads: ξ˙ = A(z, ξ)ξ + B1 satu0 (Kξ) + B2 (z, ξ)ω(t) (17) Relative to the closed-loop system (17), one has to take into account the constraints (3) and (11),
(20)
(21)
The following lemma can then be stated. Lemma 1. (Gomes da Silva Jr J.M. and Tarbouriech, 2005) Consider the nonlinearity φ(Kξ) defined in (20). If ξ ∈ S(u0 ) then the relation: φ(Kξ)0 M (φ(Kξ) + Gξ) ≤ 0
(22)
is satisfied for any diagonal positive definite matrix M ∈ <3×3 . Let us now consider a positive definite function V (ξ) > 0, ∀ξ 6= 0, with V (0) = 0, such that its time-derivative along the trajectories of the closed-loop saturating system (19) verifies V˙ (ξ) ≤ ω(t)0 ω(t) (23) for all ω(t) satisfying (9) and ∀ξ ∈ D, where D is some bounded domain inside the basin of attraction of (19). If (23) is verified, it follows that Z t V (ξ(t)) − V (ξ(0)) ≤ ω(τ )0 ω(τ )dτ (24) 0
∀ξ ∈ D and ∀t ≥ 0. From (9), we have V (ξ(t)) ≤ V (ξ(0)) + δ11 and, hence, V (ξ(0)) ≤
1 1 1 =⇒ V (ξ(t)) ≤ + ζ ζ δ1
(25)
Define the sets S1 = {ξ ∈ <6 ; V (ξ) ≤ ζ1 + δ11 } and S0 = {ξ ∈ <6 ; V (ξ) ≤ ζ1 }. Provided that
Preprints of the 5th IFAC Symposium on Robust Control Design
ROCOND'06, Toulouse, France, July 5-7, 2006
S1 is included in the basin of attraction of (19), from (25) it follows that the closed-loop system trajectories remain bounded in S1 , for any initial condition ξ(0) ∈ S0 , and any ω(t) satisfying (9). Introducing the matrices R = [ I3 0 ] and C = [ 0 I3 ], we consider the following theorem. Theorem 1. If there exist a positive definite function V (ξ) (V (ξ) > 0, ∀ξ 6= 0), a gain K, a positive definite diagonal matrix M , a matrix G and two positive scalars ζ and δ1 satisfying, for any admissible z and i = 1, 2, 3: ∂V [(A(z, ξ) + BK)ξ + Bφ(Kξ)] ∂ξ −2φ(Kξ)0 M (φ(Kξ) + Gξ) − ω 0 ω < 0 V (ξ) − ξ 0 (K(i) − G(i) )0
1 ζ
V (ξ) − ξ
1 δ1
u20(i)
V (ξ) − ξ 0 R0(i) 0
+
1 ζ
+ β2
1 1 ζ + δ C0(i) 2 1 u1(i)
R(i) ξ ≥ 0 C(i) ξ ≥ 0
(27)
(28) (29)
then the gain K and the sets S1 (V, ζ, δ1 ) = {ξ ∈ <6 ; V (ξ) ≤ ζ1 + δ11 } and S0 (V, ζ) = {ξ ∈ <6 ; V (ξ) ≤ ζ −1 } are solutions to Problem 1. Proof. The satisfaction of (27) means that the set S1 (V, ζ, δ1 ) is included in the set S(u0 ) defined as in (21). Thus, one can conclude that for any ξ ∈ S1 (V, ζ, δ1 ) the nonlinearity φ(Kξ) satisfies the sector condition (22). Moreover, the satisfaction of relations (28) and (29) means that the set S1 (V, ζ, δ1 ) is included in the set Ω(ξ) defined by (18). Thus, for any ξ ∈ S1 (V, ζ, δ1 ), the constraints C2 and C3 are respected. Consider a positive definite function V (ξ) (V (ξ) > 0, ∀ξ 6= 0). We want to prove that the time-derivative of this function satisfies (23) along the trajectories of the closedloop system (19) for all admissible nonlinearity φ(Kξ), all admissible uncertain vector z and all disturbances satisfying (9). Hence, using Lemma 1, one gets: V˙ (ξ) − ω 0 ω ≤ V˙ (ξ) − 2φ(Kξ)0 (φ(Kξ) + Gξ) − ω 0 ω
B2 (z) =
"
1/z1 0 0 0 1/z2 0 0 0 1/z3
B4 (z) =
B5 (z) =
"
"
#
; B3 =
"
;
2Y1∗ 0 0 0 2Y2∗ 0 0 0 2Y3∗
1/z1 −Y1∗ /z1 1/z1 −Y1∗ /z1 1/z2 −Y2∗ /z2 0 0 1/z3 −Y3∗ /z3 −1/z3 Y3∗ /z3
0 −1/z1 0 −1/z1 0 −1/z2 0 0 0 −1/z3 0 1/z3
#
; D(e) =
"
#
#
;
;
#
e1 0 0 0 e2 0 0 0 e3
the closed-loop system reads: ξ˙ = (R0 B1 (z)C + R0 T(2) B2 (z)R + B1 K +R0 T(3) (B3 + D(e))R)ξ + B1 φ(Kξ) +R(B4 (z) + D(e)B5 (z))ω(t)
.
(30)
Thus, from (10), B1 (z), B2 (z), B4 (z) and B5 (z) depend on two uncertain parameters p1 and p2 . It follows that these matrices belong to a polytope with 4 vertices given by the combinations of value of p1 and p2 in their definition interval: Bk (z) ∈ Co{Bkj , j = 1, ..., 4}, for k = 1, 2, 4, 5. (31) The following proposition derived from Theorem 1 can then be stated. Proposition 1. If there exist symmetric positive definite matrices W ∈ <6×6 , R1 ∈ <3×3 , a diagonal positive matrix S ∈ <3×3 , two matrices Y ∈ <3×6 and Z ∈ <3×6 , three positive scalars , ζ and δ1 satisfying 2 : W A0 + A W + B Y 1j
0 0
1j
0
1
+Y B1 + R R1 R +(u21(3) (1 + β 2 ) + β 2 )R0 R uh1(2) Bi2j RW B3 RW I3 0 SB − Z 0 B4j R 0
?
?
?
?
?
−R1
?
?
?
?
0 −I6 ?
?
?
0 0 0
<0
0 −2S ? ? 0 0 −I4 ? 0 0 B5j −I3
(32)
Thus, the satisfaction of relation (26) implies that
"
V˙ (ξ) − 2φ(Kξ)0 M (φ(Kξ) + Gξ) − ω 0 ω < 0 along the trajectories of system (19). Relations (24) and (25) are then satisfied. Therefore, when w = 0, one gets V˙ (ξ) < 0, ∀ξ(0) ∈ S1 (V, ζ, δ1 ). When w 6= 0, the closed-loop trajectories of system (19) remain bounded in S1 (V, ζ, δ1 ), for any ξ(0) ∈ S0 (V, ζ) and any disturbance satisfying (9). One can conclude that the gain K and the sets S1 (V, ζ, δ1 ) = {ξ ∈ <6 ; V (ξ) ≤ ζ1 + δ11 } and S0 (V, ζ) = {ξ ∈ <6 ; V (ξ) ≤ ζ −1 } are solutions to Problem 1. 2 Theorem 1 provides a sufficient condition to solve the control gain design. However, such a condition appears not really constructive to exhibit a suitable function V (ξ), a gain K and two scalars ζ and δ1 . The idea then consists in considering a quadratic function for V (ξ), as V (ξ) = ξ 0 P ξ,
−1/z1 Y1∗ /z1 1 + (Y1∗ )2 −1/z2 Y2∗ /z2 1 + (Y2∗ )2 −1/z3 Y3∗ /z3 1 + (Y3∗ )2
B1 (z) =
(26)
(K(i) − G(i) )ξ ≥ 0 1 δ1
P = P 0 > 0. Furthermore, we have also to write in a tractable way the matrices A(z, ξ) and B2 (z, ξ), and therefore the closed-loop system (19). In this sense, we define:" #
W ? ? Y(i) − Z(i) ζu20(i) ? Y(i) − Z(i) 0 δ1 u20(i)
" "
W ? ? R(i) W ζβ 2 ? R(i) W 0 δ1 β 2
#
W ? ? C(i) W ζu21(i) ? C(i) W 0 δ1 u21(i)
#
≥0
(33)
≥0
(34)
#
(35)
≥0
∀i = 1, 2, 3, then, the control gain K ∈ <3×6 given by K = Y W −1 is such that: (i) when ω 6= 0, the trajectories of the closed-loop system (17) remain bounded in the set o n E1 (W, ζ, δ1 ) =
2
ξ ∈ <6 ; ξ 0 W −1 ξ ≤
? stands for symmetric blocks.
1 1 + ζ δ1
(36)
Preprints of the 5th IFAC Symposium on Robust Control Design
for any ξ(0) ∈ E0 (W, ζ), n E0 (W, ζ) =
ξ ∈ <6 ; ξ 0 W −1 ξ ≤
1 ζ
ROCOND'06, Toulouse, France, July 5-7, 2006
o
(37)
and for any ω(t) satisfying (9); (ii) when ω = 0, the set E0 (W, ζ) = E1 (W, ζ, δ1 ) is included in the basin of attraction of the closedloop system (17) and is contractive. Proof. The proof mimics the one of Theorem 1. The satisfaction of relation (33) means that the set E1 (W, ζ, δ1 ) defined in (36) is included in the set S(u0 ) defined in (21). Thus, one can conclude that for any ξ ∈ E1 (W, ζ, δ1 ) the nonlinearity φ(Kξ) satisfies the sector condition (22) with G = ZW −1 . Furthermore, the satisfaction of relations (34) and (35) implies that the set E1 (W, ζ, δ1 ) is included in the sets Ω(ξ) defined in (18). Hence, for any admissible uncertain vector z (see equation (31)) and any admissible vector belonging to E1 (W, ζ, δ1 ), the closed-loop system (19) or (30) can be written as: ξ˙ =
4 X
j=1 (38) +R0 T(3) (B3 + D(e))R + B1 K]ξ +R0 (B4j + D(e)B5j )ω} + B1 φ(Kξ) 4 X with λj = 1, λj ≥ 0 , and A1j = R0 B1j C. j=1
The time-derivative of V (ξ) = ξ 0 W −1 ξ along the trajectories of system (38) writes: 4 X
λj {[A1j + R0 T(2) B2j R
j=1
+R0 T(3) (B3 + D(e))R + B1 Y W −1 ]ξ +R0 [B4j + D(e)B5j ]ω} + 2ξ 0 W −1 B1 φ(Kξ) −2φ(Kξ)0 M (φ(Kξ) + ZW −1 ξ) − ω 0 ω By convexity one can prove that the right term of the above inequality is negative definite if the following condition is verified: 0
2ξ W
−1
0
Optimization Issues It is important to note that relations (32), (33), (34) and (35) of Proposition 2 are LMIs. Depending on the energy bound on the disturbance, δ1 , is given by the designer or not, the following optimization problems can be considered: • given δ1 , we want to optimize the size of the sets E0 and E1 . This case can be addressed by the following convex optimization problem:
λj {[A1j + R0 T(2) B2j R
V˙ (ξ) − ω 0 ω ≤ 2ξ 0 W −1
Thus, the satisfaction of relations (32) to (35) with M = S −1 , K = Y W −1 and G = ZW −1 allows to verify that V˙ (ξ) < ω 0 ω (39) for all ξ(0) ∈ E0 and any ω(t) satisfying (9). Hence the trajectories of the closed-loop system (17) remain bounded in E1 (W, ζ, δ1 ) for all ξ(0) ∈ E0 (W, ζ) and any ω(t) satisfying (9). This completes the proof of statement (i). To prove statement (ii), assume that ω = 0. Then, from (39), we have V˙ (ξ) < 0, ∀ξ ∈ E1 , which means that E0 = E1 is a set of asymptotic stability for system (19). 2
0
[A1j + R T(2) B2j R + R (B3 + D(e))R +B1 Y W −1 ]ξ + 2ξ 0 W −1 B1 φ(Kξ) +2ξ 0 W −1 R0 (B4j + D(e)B5j )ω −2φ(Kξ)0 M (φ(Kξ) + ZW −1 ξ) − ω 0 ω < 0
min
W,R1 ,Y,Z,S,,ζ
ζ +δ+σ
subject to relations (32), (33), (34),(35), δI6 ? σI6 ? Y I3 ≥ 0, I6 W ≥ 0
(40)
The last two constraints are added to guarantee a satisfactory conditioning number for K and W . • δ1 being a decision variable, we want to minimize it. This problem comes to find the largest disturbance tolerance. In this case, we can add δ1 in the previous criteria; the other decision variables may be kept in order to satisfy a certain trade-off between the size of the sets E0 , E1 and δ1 . 4. APPLICATION In this section we present a result of simulation of the proposed controller in the case that the camera is mounted on a pan-platform supported by a wheeled robot (Fig. 2). x and y are the coordi-
Thus, by using (11) one can upper-bound the term containing T(2) as follows 2ξ 0 W −1 R0 T(2) B2j Rξ ≤ ξ 0 (W −1 R0 R1 RW −1 0 +u21(2) R0 B2j R1−1 B2j R)ξ with R1 = R10 > 0. In the same way, by using both (11) and (18), one can upper-bound the term containing T(3) and D(e) as follows: 2ξ 0 W −1 R0 (T(3) (B3 + D(e))Rξ +"D(e)B5j ω)#= B3 R 0 h ξ i R 0 2ξ 0 W −1 R0 T(3) I3 T(3) D(e) D(e) ω 0 B5j 2 2 2 0 −1 0 −1 ≤ (u1(3) (1 + β ) + β )ξ W R RW ξ
−1
+
ξ0 ω
0
"
B3 R 0 R 0 0 B5j
#0 "
B3 R 0 R 0 0 B5j
#
h i ξ ω
Fig. 2. Camera fixed on a pan-platform supported by a wheeled robot nates of the robot reference point M with respect to R(O, X, Y, Z), θ is the direction of the vehicle with respect to the X-axis, and θp is the direction of the pan-platform with respect to the robot’s main direction. A frame RP (P, XP , YP , ZP ) is attached to the pan-platform whose origin is at the center of rotation P . The transformation between RP and RC consists of an horizontal translation
Preprints of the 5th IFAC Symposium on Robust Control Design
ROCOND'06, Toulouse, France, July 5-7, 2006
0
of vector [ a b 0 ] and a rotation of angle π2 about the YP -axis. Dx is the distance between M and P . The velocities of the robot, which constitutes the actual system inputs are described by the vector q˙ = [v1 v2 v3 ]0 , where v1 and v2 are the linear and the angular velocities of the cart with respect to R, while v3 is the pan-platform angular velocity with respect to RM . We consider the following kinematic model: x˙ cos(θ) 0 0 " # v y˙ sin(θ) 0 0 1 θ˙ = 0 1 0 v2 v3 0 01 θ˙p
Fig. 3. Trajectory of the robot (left) and visual features evolution (right)
The kinematic screw T is linked to the velocity vector by the relation T = J(q)q, ˙ in which the robot Jacobian is given by: " # − sin(θp ) Dx cos(θp ) + a a J(q)= cos(θp ) Dx sin(θp ) − b −b (41) 0 −1 −1 As shown in (Pissard-Gibollet and Rives, 1995), the rotational degree of freedom of the pan platform allows to overcome the nonholonomic constraint. Using the proposed control method, the robot tracks correctly the target and get stabilized at the expected position, despite uncertainties on the target point depth. With respect to the absolute frame R, the coordinates of the target points are: E1 (10m, 0.5m), E2 (10m, 0m) and E3 (10m, −0.5m). The interval of distance between the robot and camera is d ∈ [2.454m, 8m]. At the expected position, the visual features are defined by: Y1∗ = 0.2, Y2∗ = 0, and Y3∗ = −0.2. To guarantee the visibility we considered β = 0.4. The initial robot’s configuration is given by: x = 4.85m, y = −0.8m, θ0 = 0rad and θp = 0.175rad, and the initial value of the state vector is: ξ(0) = [−0.12m; −0.0159m; 0.0849m; 0ms−1 ; 0ms−1 ; 0 rad−1 ]. The bounds on the camera velocity and acceleration are u1 = [1 1 0.1]0 , and u0 = [1 1 5]0 . By applying the proposed control scheme with the Matlab LMI Control Toolbox we obtained the following value for the control gain K: " # K=
−505 1392.8 −507.8 −6.5 0.1 8.6 −814 35.1 784.8 0.1 −12.5 0.1 −937.5 −161.2 −973.5 8.6 0.1 −72.8
This value of K was obtained for the scalar δ1 = 12.1949. Fig. 3 represents the robot trajectory and the evolution of the visual features. The velocities of the robot and the kinematic screw of the camera are described in Fig. 4. One can see in Fig. 5 that the second components saturate during the very beginning of the task. REFERENCES Bellot, D. and P. Dan`es (2001). Handling visual servoing schemes through rational systems and lmis. IEEE Int. Conference on Decision and Control 4, 3601–3606. Cuvillon, L., E. Laroche, J. Gangloff and M. de Mathelin (2005). GPC versus H∞ control for fast visual servoing of medical manipulator including flexibilities. IEEE Int. Conference on Robotics and Automation.
Fig. 4. Robot’ velocities (left), and camera’s kinematic screw T (right)
Fig. 5. Description of the control components during the very beginning of the task) Espiau, B., F. Chaumette and P. Rives (1992). A new approach to visual servoing in robotics. IEEE Trans. on Robotics and Automation. Gomes da Silva Jr J.M. and S. Tarbouriech (2005). Anti-windup design with guaranteed regions of stability: an lmi-based approach. IEEE Trans. Autom. Control 50(1), 106–111. Malis, E. and P. Rives (2003). Robustness of image-based visual servoing with respect to depth distribution errors. In: IEEE Int. Conf. on Robotics and Automation. Taipei Taiwan. Marchand, E. and F. Chaumette (2005). Feature tracking for visual servoing purposes. Robotics and Autonomous Systems. Mezouar, Y. and F. Chaumette (2002). Path planning for robust image-based control. IEEE Trans. On Robotics and Automation 18(4), 534–549. Pissard-Gibollet, R. and P. Rives (1995). Applying visual servoing techniques to control a mobile hand-eye system. In: IEEE Int Conf. on Robotics and Automation. Nagoya, Japan. Tarbouriech, S. and P. Sou`eres (2000). Advanced control strategies for the visual servoing scheme. IFAC Int. Symp. on Robot Control. Zachi, A.R.L., L. Hsu and F. Lizarralde (2004). Performing stable 2d adaptive visual positionning/tracking control without explicit depth measurement. IEEE Int. Conf. on Robotics and Automation.