A Visual Servo Control based on Geometric Algebra

A Visual Servo Control based on Geometric Algebra C. L´opez-Franco, Geoff Fink, N. Arana-Daniel and A. Y. Alanis Computer Science Department, CUCEI, University of Guadalajara

Abstract— This paper presents a new visual servo controller based on geometric algebra (GA). The controller has an angular and translational components that are designed using rotors and translators. The controller does not require additional calculations or jacobian-like matrices. The proposed adaptive controller regulates a camera to a desired position and orientation from a desired image. Lastly, we develop a GA-based Lyapunov function to demonstrate the stability of the controller and we present simulations validating the proposed controller.

relationship between images points taken at different views of a 3D point is also presented. A. Modeling and notations Let P be a point in the 3D space. Its 3D coordinates ~ r = [Xr Yr Zr ] in the reference frame Fr . The image are X ~ r can be found using the perspective point ~xr of the point X ~ r is projected on a virtual projection model. The point X plane perpendicular to the optical axis with

Keywords: Geometric algebra, visual servo control I. Introduction Visual servo control refers to the use of computer vision data to control the motion of a robot. The camera is generally mounted on a robot manipulator or a mobile robot, and thus the motion of the robot induces motion on the camera. Commonly, a desired image is taken previously to the control loop, and in the control loop the visual data is extracted from the current camera image to compute an error. This error is used by the controller to move the robot from its current position to the desired position, the control loop ends when the current image is similar to the desired image, in terms of the predefined error. In this paper we introduce a novel controller based on the geometric algebra (GA). The geometric algebra is a coordinate-free approach to geometry, unlike conventional vector algebra, has available a fully invertible, associative product between vectors. This is achieved via the geometric product and the capability of adding together object of different type, such scalars and vectors, in the same way as a complex number is build out of imaginary and real parts. The GA framework has a tremendous power given by the geometric product, and its applications spread across many areas of mathematics, physics and engineering. This paper is organized as follows. In section II we give a brief introduction to the techniques of computer vision used in this work. In section III we present an introduction to the geometric algebra. In section IV the proposed controller and its stability analysis is given. In section V the simulations results using the proposed controller are given. Finally, the conclusions are in section VI.

~xr =

1 ~ Xr Zr

(1)

where ~xr = [xr yr 1]. The point ~xr lies on the reference image Ir in normalized coordinates. The image coordinates p~r = [ur vr ] can be obtained from the normalized coordinates with the transformation p~r = K~xr

(2)

where the matrix K represents the camera intrinsic calibration parameters, which can be written as   f f s u0 K =  0 f r v0  (3) 0 0 1 where f is the focal length in pixels, u0 and v0 are the coordinates of the principal point (in pixels), r is the aspect ratio and s represents the skew factor. Given two frames Fc and Fr , the rigid transformation between them can be represented with a rotation R ∈ SO(3) and translation ~t ∈ R3 . In the current frame Fc ~ c = [Xc Yc Zc ] and is the point P has the coordinates X ~ c with related with X ~ c = RX ~ r + ~t X

(4)

~ c projects on the current normalized image The point X Ic in ~xc = [xc yc 1] with ~xc =

1 ~ Xc Zc

(5)

II. Computer vision model

and project on the current image with the transformation

In this section we give a brief introduction to the image formation process of a 3D point. Additionally, the

p~c = K~xc

(6)

B. Relationship between two images of a plane

P

The relationship between the image points p~r and p~c is given by a linear transformation called homography [1], [2], [3], [4]. Let us suppose that the point P belongs to a plane π. Let ~nr and dr be the normal and distance to π ~ r is expressed in the reference frame Fr . Since the point X on π it satisfies ~r ~nTr X

= dr

(7)

p

xc

xr H

Fc

Fr

which is equivalent to 1 T~ ~n Xr = 1 dr r

(8)

Fig. 1. Projection model and homography between two images of a plane

Now substituting (1), (5) and (8) into (4) we obtain Zc ~xc = H~xr Zr

(9)

where H denotes the homography matrix (Fig. 1), which can be written as H=R+

~t ~nTr dr

(10)

Now using (2), (6) and (9) we obtain the following relationship Zc ~xc = H~xr Zr

(11)

Finally, substituting (2) and (6) into the previous equation we get Zc p~c = G~ pr Zr

(12)

where the matrix G can be written as follows: G = KHK−1

(13)

Given two images Ir and Ic of a planar target, the homography matrix can be computed up to scale of factor with four non collinear matched points. Then using an approximation of the calibration matrix K, the matrix H can be computed up to scale factor. The matrix H can be decomposed to obtain the rotation R and the normalizaed translation [1], [2], [3], [4]. III. Geometric algebra In this section we introduce the mathematical framework of geometric algebra, which will be used in the definition of the proposed controller. The elements of the geometric algebra Gp,q are called multivectors. We assume that Gp,q is algebraically closed, i.e. the sum or product of any pair of multivectors is a unique multivector. The geometric product (denoted by multivector juxtaposition) and sum of multivectors A, B, C have the following properties:

A+B (A + B) + C (AB)C (A + B)C A(B + C) A+0 1A

= = = = = = =

B+A A + (B + C) A(BC) AC + BC AB + AC A A

(14) (15) (16) (17) (18) (19) (20)

A + (−A)

=

0

(21)

Equation (14) states that multivector addition is commutative. Equations (15) and (16) indicate that the multiplication and addition of multivectors is associative. Equations (17) and (18) show that the multiplication is distributive with respect to addition. Equations (19) and (20) present the additive and multiplicative identities. Finally, equation (21) shows that every multivector A has a unique additive inverse −A. One of the basis types of multivectors is called blade. A basis blade Ar in Gp,q is defined as the geometric product of r different vector basis. Ar = a1 a2 . . . ar ,

(22)

where ai aj = −aj ai for i, j = 1, 2, . . . , r and i 6= j. A linear combination of r-blades is called r-vector Ar = A0r + A1r + A2r + · · · =

X

Air

i

The terms scalar, vector, bivector (Fig. 2), trivector, etc. are often used as alternative names of 0-vector, 1-vector, 2-vector, 3-vector, etc. respectively. Every multivector A can be written as the sum A = hAi0 + hAi1 + hAi2 + · · · =

X r

where hAir denotes the r-vector part of A.

hAir

basis e1 , e2 , e3 and two extra basis e+ , e− . From (23) we can note that the square of the first four basis is 1 and the square of e− is −1. With the two extra basis e+ and e− we define two useful vector basis

b a?b

e1

a e0

e2

e∞ Fig. 2. The bivector represents the area of the parallelogram spanned by the vectors a and b.

A. Geometric, inner and outer products The geometric product of basis vectors satisfies   1 −1 ei ej =  eij = −ej ei

for i = j ∈ {1, 2, . . . , p} for i = j ∈ {p + 1, . . . , n} for i 6= j

(23)

Given two r-blades Ar and Bs of grades r and s respectively, the geometric product between them is defined by

e− − e+ , 2 = e− + e+ , =

(27) (28)

where the vector e0 is interpreted as the coordinate system origin, and the vector e∞ as the point at infinity. The properties of e0 and e∞ can be derived from the properties of e− and e+ : e20 = e2∞

=

0

e0 · e∞

=

−1 ,

(29) (30)

Additionally, the geometric product of e+ and e− produces a special bivector called the E-plane, E = e+ e−

(31)

2

Ar Bs = hAr Bs ir+s−2k ,

(24)

where k is the number of vectors in common. Given two blades Ar and Bs of grades r and s respectively, the inner product between them is defined as ½ Ar · Bs =

hAr Bs i|r−s| 0

if r, s > 0 if r = 0 or s = 0

(25)

The outer product between two blades Ar and Bs is Ar ∧ Bs = hAr Bs ir+s .

(26)

Thus, if the grade of the outer product between Ar and Bs is r + s then Ar ∧ Bs = hAr Bs ir+s . Otherwise, the outer product is zero. Therefore the outer product of two blades Ar and Bs is zero if and only if the blades have at least on common base vector. The geometric, inner and outer products can be extended by linearity to r-vectors and thereby to multivectors. An important blade of any geometric algebra is the pseudoscalar. The pseudoscalar n-blade of the geometric algebra Gp,q , where n = p + q, is the maximal grade blade of such geometric algebra. The pseudoscalar is denoted with Ip,q . The inverse of a pseudoscalar differs from the I by almost a sign. For example, the psuedoscalar of G3 is I3 = e1 e2 e3 and its inverse is I3−1 = −e1 e2 e3 . B. Conformal Geometric Algebra The conformal geometric algebra (CGA) embeds the GA of the Euclidean space Gp into a higher dimensional space Gp+1,1 [5]. Thus the 3D Euclidean space geometric algebra G3 is embedded into the CGA G4,1 . Therefore the CGA has five orthogonal vector basis, the three Euclidean

with the following property E = 1. Any vector a in G4,1 can be defined as a = a + αe∞ + βe0

(32)

To define a bijective mapping between an Euclidean vector a ∈ G3 and the vector a ∈ G4,1 we need two constraints: a2 = 0 and a · e∞ = −1, applying this constraints to the above equation we obtain 1 a = a + a 2 e∞ + e0 (33) 2 Given a vector a ∈ G4,1 we can recover the Euclidean vector a ∈ G3 with a = (a ∧ E)E

(34)

C. Rigid body motion In CGA the rotation of entities is performed using a multivector called rotor. A rotor is an entity which is able to rotate CGA entities and is equal to ¶ θ(t) L 2 θ(t) θ(t) = cos( ) + L sin( ) (35) 2 2 where the bivector L represents the dual of the rotation vector axis l, i.e. L = lI3−1 . To rotate an entity, we simply multiply it by the rotor R from the left and by the inverse of the rotor R−1 from the right, that is µ

R(t) =

exp

x(t) = R(t)x0 R−1 (t).

(36)

The translation of entities is performed using a multivector called translator defined as

µ T = exp

e∞ t(t) 2

IV. Control

¶ (37)

To translate an entity, we simply multiply it by the translator T from the left and by the inverse of the translator T −1 from the right, that is x(t) = T (t)x0 T −1 (t).

(38)

Finally the rigid motion of entities can be handled with a multivector called motor and is defined as M (t)

= T (t)R(t) µ ¶ ¶ µ e∞ t(t) θ(t) = exp L exp 2 2

The control objective is to drive a camera mounted on a robot to a desired position and orientation (Fig. 3). Given the desired and current images, it is possible to compute the homography matrix. The matrix G can be computed up to a scale factor with four non collinear matched points. Then using the camera calibration matrix K we can compute the homography matrix H up to a scale factor. Later, the matrix H can be decomposed to recover the rotation matrix R and the translation vector ~t. Finally, the rotor R and the translator T can be computed from the matrix R and the vector ~t respectively. The control objective can be qualified as

(39)

The rigid motion of an entity due to M can be computed

R(t) → 1 as t → ∞

(46)

T (t) → 1 as t → ∞.

(47)

and

as x(t) = M (t)x0 M −1 (t).

(40)

D. Kinematics The velocity of a multivector can be defined as the derivative of (40), that is x(t) ˙ = M˙ (t)x0 M −1 (t) + M (t)x0 M˙ −1 (t).

1 V M (t) 2

Desired

(41)

The derivative of M can be found by taking the derivative of (39) M˙ (t) =

a)

Current c)

b)

(42)

where V (t)

˙ ˙ + θ(t)L = e∞ t(t) = e∞ v(t) + ω(t)L

(43)

where v and ω represent the linear and angular velocity vectors respectively. Similarly, the derivative of M −1 (t) can be found to be

Fig. 3. An example of positioning task. a) The 3D scenario with the desired and current camera poses, b) The desired image of the target and c) The current image of the target.

Given a motor M computed from the homography H we can extract the rotor R from the motor with R(t) = (M (t) ∧ E)E

M˙ (t) =

1 − M −1 (t)V 2

(44)

Substituting (42) and (44) into (41) and rewriting x(t) ˙

¢ 1¡ V (t)M (t)x0 M −1 (t) − M (t)x0 M −1 (t)V (t) 2 1 (V (t)x(t) − x(t)V (t)) = 2 = V (t) · x(t) (45)

=

For more details on geometric algebra and CGA, the interested reader is referred to view ([6]), ([7]), ([5]), [8], [9], [10].

(48)

and the translator with T (t) = M (t)R−1 (t)

(49)

From (48) we define an angular velocity controller as ­ ® ω(t) = λω R−1 (t) 2 I3

(50)

where λω is a constant gain. The linear velocity controller is defined as ­ ® v(t) = λv T −1 (t) 2 · e0

(51)

µ

0

= = = =

and

1 1 θ θ˙ − sin 2 2 µ ¶ 1 hRi2 ˙ − θ 2 L µ ¶ 1 hRi2 ³ ω ´ − 2 L L ω − 2 hR2 i L

(53)

and

D E 1˙ 1 T˙ = te (56) ∞ = v 2 2 2 Thus, from (53-56) the time derivative of A(t) can be determined as follows

= =

20 30 Time (s)

40

−0.06 −0.08

50

Error (m)

Velocity (rad/s)

−0.04

x y z

0.5 0 −0.5

10

20 30 Time (s)

40

−1

50

10

20 30 Time (s)

40

50

−0.5

−1

D E D E D E −2(1 − hRi0 ) R˙ − 2 hRi2 R˙ + 2 hT i2 T˙ 2

1 (1 − hRi0 ) hRi2 ω − hRi2 ω + hT i2 v L2 1 (1 − hRi0 ) hRi2 λω hRi2 L2 − hRi2 λω hRi2 + hT i2 λv hT i2

10

20 30 Time (s)

40

50

1

0 −0.5 −1

0

x y z

0.5

x y z

−1 Error (m)

x y z

Velocity (rad/s)

0

−2 −3 −4

10

20 30 Time (s)

40

50

−5

10

20 30 Time (s)

40

50

Fig. 5. The linear (left) and angular (centre) velocity and error (right) of a translation of 5m along the x axis

0

0

10

1

x y z

Fig. 4. The linear (left) and angular (centre) velocity and error (right) of a rotation of 45◦ about the ]x axis

−1.5

D E 1˙ 1 R˙ = θI 3 = ω 2 2 2

=

−1

¶

(54) D E Similarly, given that T = hT i0 + hT i2 and T˙ = T˙ + 0 D E from (37) we derive T˙ 2 D E T˙ = 0 (55)

A˙

0

0 −0.02

2

A(t) = (1 − hRi0 )2 − hRi2 + hT i0 + hT · e0 i2 (52) D E D E Given that R = hRi0 + hRi2 and R˙ = R˙ + R˙ 0 2 from (35) we derive D E R˙

x y z

0.5

−0.5

Velocity (m/s)

2

1

Velocity (m/s)

A. Stability The controller in (50) and (51) ensures global asymptotic translation and rotation regulation, i.e. kM (t)k → 1, kR(t)k → 1, kT (t)k → 1, kV (t)k → 0 as t → ∞. Proof: Let A(t) ∈ G4,1 denote the following non-negative positive definite function (i.e. a Lyapunov candidate):

2

(57)

From (52) and (57) and by Lyapunov stability the controllers in (50) and (51) are globally asymptotically stable. V. Experimental Results To validate the new controller we performed a series of simulations. The experimental setup included a robot with six DOF and a camera mounted on its end effector. The camera calibration matrix used was   100 0 320 K =  0 100 240  0 0 1

and the constants gains used in the controller were λv = 0.5 and λw = 0.2. For simplicity every iteration of the simulation was 1s. Initially, a desired image was calculated and the robot had to translate and rotate to re-create the desired image. In the first simulations, the robot was rotated by 45◦ about one axis. The linear and angular velocity and error of a rotation about the x axis are shown in Fig. 4. The second set of simulations were a translation along one axis by 5m. The linear and angular velocity and error of the translations in the x are shown in Fig. 5. The rotations and translations about y and z axes are not shown as the results are exactly the same as the x axis. The third set of simulations were a translation of 5m in the x, −3m in the y and 3m in the z along with a rotation of 45◦ along the z axis followed by a rotation of 60◦ along the y axis and finally 90◦ along the x. The velocity in Fig. 6 is based on the GA controller given in this paper and Fig. 7 is based on a quaternion controller [11]. Figures 6 and 7 also display the translational error in the x, y and z. Lastly, in Figures 8 and 9 show the linear velocity and error, respectively, of a translation of 5m along the z axis when ² = 0.00001, ² = 0.1 and ² = 1 using the motor-based controller. Where ² represents the minimal distance between the camera and the object. Figures 10 and 11 show the same simulations, but with the quaternion-based controller. We can notice that the quaternion-based controller involves a depth ratio which has problems with the use of ² less than one, but the motor-based controller never had this problem. VI. Conclusions In this paper we presented a visual servo controller based on geometric algebra. We design an angular velocity controller that has no singularities, and a translation controller using the same entity in CGA as the angular velocity controller without the need for additional calculations

−0.5

−0.05 −0.1

−1 −1.5

10

20 30 Time (s)

40

−0.15

50

2

10

20 30 Time (s)

40

0

−4

−4

−5

10

20 30 Time (s)

40

−0.2

−0.05

10

20 30 Time (s)

40

−0.15

50

x y z

2

−0.1

−0.3 −0.4

4

x y z

0

Error (m)

Velocity (m/s)

−0.1

0.05

Velocity (rad/s)

x y z

0

0 −2 −4

10

20 30 Time (s)

40

−6

50

10

20 30 Time (s)

40

50

Fig. 7. The linear (left) and angular (centre) velocity and error (right) of a quaternion-based controller using multiple rotations and translations

−0.5

−1

−1.5

10

20 30 Time (s)

40

0

x y z

−0.5

−1

−1.5

50

10

20 30 Time (s)

40

0

Velocity (m/s)

x y z

Velocity (m/s)

Velocity (m/s)

0

x y z

−0.5

−1

−1.5

50

10

20 30 Time (s)

40

50

Fig. 8. The linear velocity of a translation along the z axis when ² = 1 (left), ² = 0.1 (centre) and ² = 0.00001 (right) with a motorbased controller

−2 −3 −4

0

x y z

−1 Error (m)

Error (m)

0

x y z

−2 −3 −4

−5

10

20 30 Time (s)

40

−2 −3 −4

−5

50

x y z

−1 Error (m)

0 −1

10

20 30 Time (s)

40

−5

50

10

20 30 Time (s)

40

50

Fig. 9. The error of a translation along the z axis when ² = 1 (left), ² = 0.1 (centre) and ² = 0.00001 (right) with a motor-based controller

−0.1 −0.15 −0.2

0

x y z

−0.05 −0.1 −0.15

10

20 30 Time (s)

40

50

−0.2

0

Velocity (m/s)

x y z

Velocity (m/s)

Velocity (m/s)

0 −0.05

x y z

−0.05 −0.1 −0.15

10

20 30 Time (s)

40

50

−0.2

10

20 30 Time (s)

40

−2 −3 −4

10

20 30 Time (s)

40

50

−5

x y z

−1 −2 −3 −4

10

20 30 Time (s)

40

50

−5

10

20 30 Time (s)

40

50

50

Fig. 6. The linear (left) and angular (centre) velocity and error (right) of a motor-based controller using multiple rotations and translation

0.1

−3

−2

−6

50

−2

0

x y z

−1 Error (m)

0

Error (m)

0

x y z

0

x y z

−1

4

x y z Error (m)

Velocity (m/s)

0.5

0.05

Velocity (rad/s)

x y z

Error (m)

0 1

50

Fig. 10. The linear velocity of a translation along the z axis when ² = 1 (left), ² = 0.1 (centre) and ² = 0.00001 (right) with a quaternionbased controller

Fig. 11. The error of a translation along the z axis when ² = 1 (left), ² = 0.1 (centre) and ² = 0.00001 (right) with a quaternionbased controller

or jacobian-like matrices. Several simulations experiments have been carried out to show that the controller regulates a camera to a desired position and orientation from a desired image. Lastly, we develop a CGA-based Lyapunov function to demonstrate the stability of the controller and we present simulations validating the controller. Acknowledgments We are thankful to CONACYT, PROMEP and the University of Guadalajara for supporting this work. References [1] Olivier Faugeras and F. Lustman. Motion and Structure from Motion in a Piecewise Planar Environment. 0 RR-0856, INRIA, June 1988. [2] Zhongfei Zhang and A.R. Hanson. Scaled Euclidean 3D Reconstruction Based on Externally Uncalibrated Cameras. In Computer Vision, 1995. Proceedings., International Symposium on, pages 37 –42, Nov 1995. [3] Y. Ma. An Invitation to 3-D Vision: From Images to Geometric Models. Interdisciplinary applied mathematics: Imaging, vision, and graphics. Springer, 2004. [4] R. Hartley and A. Zisserman. Multiple View Geometry in Computer Vision. Cambridge University Press, 2003. [5] H. Li and D. Hestenes. Generalized homogeneous coordinates for computational geometry. In G. Sommer, editor, Geometric Computing with Clifford Algebra, volume 24, pages 27–60, Berlin Heidelberg, 2001. Springer-Verlag. [6] D. Hestenes, H. Li, and A. Rockwood. New algebraic tools for classical geometry. In G. Sommer, editor, Geometric Computing with Clifford Algebra, volume 24, pages 3–26, Berlin Heidelberg, 2001. Springer-Verlag. [7] E. Bayro-Corrochano. Robot perception and action using conformal geometric algebra. Springer-Verlag, Heidelberg, 2005. [8] C. Perwass. Geometric Algebra with Applications in Engineering. Springer-Verlag, Heidelberg, 2009. [9] E. Bayro-Corrochano, L. Reyes-Lozano, and J. ZamoraEsquivel. Conformal geometric algebra for robotic vision. Journal of Mathematical Imaging and Vision, 24, 2006. [10] L. Dorst, D. Fontijne, and S. Mann. Geometric Algebra for Computer Science: An Object-Oriented Approach to Geometry. Morgan Kaufman, Heidelberg, 2007. [11] G. Hu, W.E. Dixon, S. Gupta, and N. Fitz-Coy. A Quaternion Formulation for Homography-based Visual Servo Control. In Robotics and Automation, 2006. ICRA 2006. Proceedings 2006 IEEE International Conference on, pages 2391 –2396, May 2006.