Visual Feedback Systems with a Fixed Camera

ugliestmysticΤεχνίτη Νοημοσύνη και Ρομποτική

14 Νοε 2013 (πριν από 3 χρόνια και 1 μήνα)

47 εμφανίσεις

Systems & Control Laboratory

Kanazawa University

Kanazawa University

1

Passivity
-
based Control and Estimation of

Visual Feedback Systems with a Fixed Camera


University of Karlsruhe

September 30
th
, 2004


Masayuki Fujita


Department of Electrical and Electronic Engineering

Kanazawa University, Japan

Systems & Control Laboratory

Kanazawa University

Kanazawa University

2

Application of Visual Feedback System

Fig. 2: Some Examples of

Visual Feedback System



Swing Automation for Dragline



Autonomous Injection of Biological Cells
etc.

Introduction



Automatic Laparoscope (Surgical Robot)
etc.



Eye
-
in
-
Hand Configuration



Fixed
-
camera Configuration

Fig. 1(a): Eye
-
in
-
Hand

Camera

Image

Robot

Target Object

World Frame

Camera

Image

Robot

Target Object

World

Frame

Fig. 1(a): Fixed
-
camera

In the previous work, almost all the proposed methods

depend on the camera configuration.

Our proposed methods have the same strategy

for both camera configurations.

One of methods for the eye
-
in
-
hand configuration

has been discussed in ACC, 2003. In this research,

it will be extended to the fixed
-
camera configuration.

Systems & Control Laboratory

Kanazawa University

Kanazawa University

3

4
4
ˆ
1
0











R
ab
ab
p
e
g
ab


3
R

ab
p
)
3
(
ˆ
SO
e
ab



Position and Orientation

Homogeneous Representation

Fig. 3: Visual Feedback System

Objective of Visual Feedback Control

ho
g
w

o

c

3
R

ab

R

ab

Direction of Rotation:

Angle of Rotation:

Rodrigues’ formula

h

Objective of Visual Feedback Control

One of the control objective is to track

the target object in the 3D workspace.

The relative rigid body motion

must tend to the desired one

d
g
ho
g
ho
co
wo
g
g
g
,
,
Unknown Information

( Target motion is unknown
.)


wo
g
ho
g
co
g
ch
wh
wc
g
g
g
,
,
Known Information

)
(
1
wh
wc
ch
g
g
g



Composition Rule

wh
g
ch
g
wc
g
Systems & Control Laboratory

Kanazawa University

Kanazawa University

4

Fundamental Representation

Fig. 3: Visual Feedback System

wc
g
wo
g
co
g
w

o

c

h

b
wo
b
wc
g
b
co
V
V
V
co




)
(
1
Ad
Fundamental Representation

of Relative Rigid Body Motion

Body Velocity of Camera

:
b
wc
V
:
b
wo
V
Body Velocity of Target Object
















ˆ
ˆ
ˆ
)
(
0
ˆ
Ad
e
e
p
e
g
6
R








ab
ab
b
ab
v
V

: Translation

: Orientation


is the difference between and .

b
co
V
b
wc
V
b
wo
V
Relative Rigid Body Motion

in Fixed Camera Configuration : OMFC

b
wo
b
co
V
V

( Camera is static.
)


0

b
wc
V
b
wo
V
co
g
not measurable

OMFC

Fig. 4: Block Diagram of OMFC

(Object Motion from Camera)

(1)

(2)

Systems & Control Laboratory

Kanazawa University

Kanazawa University

5

Target Object

ci
p
p
oi
p
i
f
w

o

c

World Frame


Camera Frame

Image Plane

: Focal Length




T
ci
ci
ci
z
y
x
 

:
Perspective Projection

Relative Feature Points








ci
ci
ci
i
y
x
z
f

Fig. 5: Pinhole Camera

Image Information
(
m

points)












m
f
f
f

1
Image information
f

includes the relative rigid body motion .

Fig. 6: Block Diagram of OMFC with Camera

)
,
(
ˆ
co
e
p
g
co
co



Camera Model and Image Information

ci
p
)
(
ˆ
co
e
g
co





co
p
oi
p
oi
p
( depends on .)

i
f
ci
p
f
measurable

b
wo
V
co
g
not measurable

OMFC

Camera

(3)

(4)

(5)

Systems & Control Laboratory

Kanazawa University

Kanazawa University

6

e
b
co
u
V

Model of Estimated Relative Rigid

Body Motion

䕳位EC

)
,
(
ˆ
co
e
p
g
co
co



oi
co
ci
p
g
p

Estimated OMFC

e
u
: Input for Estimation Error

Estimated Image Information

Fig. 7: Block Diagram of Estimated OMFC

Nonlinear Observer in VFS

Estimated Body Velocity

Fig. 3: Visual Feedback System

wc
g
wo
g
co
g
w

o

c

h

b
wo
b
co
V
V

estimated

f
co
g
EsOMFC

Camera

Model

e
u
(6)

(2)








ci
ci
ci
i
y
x
z
f

:
(7)

(8)

Systems & Control Laboratory

Kanazawa University

Kanazawa University

7

Fig. 8: Block Diagram of OMFC and Estimated OMFC

Nonlinear Observer in VFS continued

)
,
(
ˆ
ee
e
p
g
ee
ee



co
co
ee
g
g
g
1
:


Estimation Error








)
(
:
ˆ
ee
e
e
p
e
R
ee
e


(Vector Form)

e
e
g
J
f
f
)
(


Relation between Estimation Error

and Image Information

f
measurable

b
wo
V
co
g
not measurable

OMFC

Camera

+

e
e

J
e
u
f
co
g
EsOMFC

Camera

Model

estimated

estimated

(Error between Estimated State and Actual One)

Fig. 3: Visual Feedback System

wc
g
wo
g
co
g
w

o

c

h

estimated

(9)

Estimation Error System

b
wo
e
g
b
ee
V
u
V
ee




)
(
1
Ad
(10)

Systems & Control Laboratory

Kanazawa University

Kanazawa University

8

)
,
(
ˆ
d
e
p
g
d
d










)
(
:
ˆ
ec
e
e
p
e
R
ec
c


)
,
(
ˆ
ec
e
p
g
ec
ec



ho
d
ec
g
g
g
1
:


Desired RRBM

Control Error

Fig. 9: Block Diagram of HMFC and Reference

Control Error System

(Vector Form)

(Error between Estimated State and Desired One)

Fig. 3: Visual Feedback System

ho
g
co
g
w

o

c

h

ch
g
ho
co
g
g
,
Estimated Information

)
:
(
1
co
ch
ho
g
g
g


by Composition Rule

e
b
wh
g
b
ho
u
V
V
ho




)
(
1
Ad
Relative Rigid Body Motion from to

h

o

(HMFC:
Hand Motion from Camera
)

(11)

b
d
g
e
b
wh
g
b
ec
V
u
V
V
ec
ho
)
(
)
(
1
1
Ad
Ad






Control Error System

(This is dual to the estimation error system.)

(12)

e
u
b
wh
V
ho
g
d
g
c
e
HMFC

+

Systems & Control Laboratory

Kanazawa University

Kanazawa University

9

Control Error System

Estimation Error System

Visual Feedback System with Fixed Camera Configuration

Visual Feedback System

b
wo
ce
b
ee
b
ec
V
I
u
V
V




















0
)
(
1
Ad


ho
g
)
(
1
Ad


ee
g
I
0








e
b
d
g
b
wh
ce
u
V
V
u
d
)
(
Ad
:







e
c
e
e
e
:
Fig. 10: Block Diagram of Control and Estimation Error Systems

b
wo
V
f
co
g
OMFC

Camera

e
u
f
co
g
EsOMFC

Camera

Model

+


J
e
e
(13)

d
g
c
e
b
wh
V
ho
g
HMFC

+

Systems & Control Laboratory

Kanazawa University

Kanazawa University

10

Visual Feedback System with Fixed Camera Configuration

Visual Feedback System

b
wo
ce
b
ee
b
ec
V
I
u
V
V




















0
)
(
1
Ad


ho
g
)
(
1
Ad


ee
g
I
0
Fig. 11: Block Diagram of Visual Feedback System

Controller

ce
u
+

b
d
g
V
d
)
(
Ad
b
wo
V
f
co
g
OMFC

Camera

e
u
f
co
g
EsOMFC

Camera

Model

+


J
e
e
(13)









e
b
d
g
b
wh
ce
u
V
V
u
d
)
(
Ad
:







e
c
e
e
e
:
d
g
c
e
b
wh
V
ho
g
HMFC

+

Systems & Control Laboratory

Kanazawa University

Kanazawa University

11

Property of Visual Feedback System

Fig. 11: Block Diagram of Visual Feedback System

Controller

ce
u
+

b
d
g
V
d
)
(
Ad
f
co
g
e
u
f
co
g
Camera

Model

d
g
c
e
b
wh
V
ho
g
+


J
e
e
Lemma 1

If the target is static , then the visual feedback

system (13) satisfies

ce
ce
T
T
ce
d
u






0
)
0
(

b
wo
V
ce

,
where

is a positive scalar.















I
N
e
N
ec
d
e
T
g
ce
ce
ce
)
(
)
(
ˆ
1
Ad
0
Ad
:
,
:



OMFC

Camera

EsOMFC

HMFC

ce

ce
N
(14)

+

Systems & Control Laboratory

Kanazawa University

Kanazawa University

12

(Proof)

Differentiating the energy function (15) with respect to time along

the trajectories of the visual feedback system yields

)
,
(
2
)
(
q
q
C
q
M



ee
we
T
ee
ec
wc
T
ec
ce
e
g
T
p
p
p
p
u
I
e
V
ec
d




ˆ
ˆ
0
Ad
Ad
)
(
)
(
ˆ
1













ce
T
ce
u


is

a skew
-
symmetric matrix

skew
-
symmetric matrices

Integrating both sides from 0 to
T
, we can obtain

ce
ce
T
T
ce
V
V
T
V
d
u










:
)
0
(
)
0
(
)
(
0
(14)

Passivity Property of Manipulator Dynamics

T
ce


(Q.E.D.)

Property of Visual Feedback System continued

)
(
2
1
)
(
2
1
ˆ
2
ˆ
2
ee
ec
e
p
e
p
V
ee
ec










Energy Function

(15)

)
(
tr
2
1
:
)
(
ˆ
ˆ





e
I
e


Error Function of Rotation Matrix

Systems & Control Laboratory

Kanazawa University

Kanazawa University

13

If , then the equilibrium point for the closed

-
loop system (14) and (16) is asymptotically stable.

0

e
Passivity
-
based Visual Feedback Control Law

Stability Analysis

Theorem 1

)
(
ce
ce
ce
ce
ce
K
e
N
K
u





0
:








ce
K
c
K
e
K
0
0
Gain

Fig. 11: Block Diagram of Visual Feedback System

Controller

+

b
d
g
V
d
)
(
Ad
f
co
g
e
u
f
co
g
Camera

Model

d
g
c
e
b
wh
V
ho
g
+


J
e
e
OMFC

Camera

EsOMFC

HMFC

ce
N
ce
u
ce

ce
K
(16)

0

b
wo
V
+

Systems & Control Laboratory

Kanazawa University

Kanazawa University

14

Tracking Problem

L
2
-
gain Performance Analysis

Based on the dissipative systems theory, we consider
L
2
-
gain performance

analysis in one of the typical problems in the visual feedback system.

Disturbance Attenuation Problem

Fig. 12: Generalized Plant of Visual Feedback System

ce
u
b
wo
V
+

f
co
g
e
u
f
co
g
b
wh
V
ho
g
+

OMFC

Camera

EsOMFC

HMFC

,
Ad
)
(
b
d
g
V
d
d
g
ce
K
ce
N

J
e
+

z
Camera

Model

Systems & Control Laboratory

Kanazawa University

Kanazawa University

15

Tracking Problem

L
2
-
gain Performance Analysis

Based on the dissipative systems theory, we consider
L
2
-
gain performance

analysis in one of the typical problems in the visual feedback system.

Disturbance Attenuation Problem

Given a positive scalar and consider the control input (31)

with the gains and such that the matrix
P

is positive

semi
-
definite, then the closed
-
loop system (28) and (31)

has
L
2
-
gain .

c
K
e
K



Theorem 2

}
,
0
{
diag
:
,
2
1
2
1
:
2
I
W
I
W
N
K
N
P
ce
ce
T
ce







represents a disturbance attenuation level of the visual feedback system.




Other problems can be considered by constructing the adequate generalized plant.

Systems & Control Laboratory

Kanazawa University

Kanazawa University

16

Experimental Testbed on 2DOF Manipulator

Systems & Control Laboratory

Kanazawa University

Kanazawa University

17



Stability Analysis



L
2
-
gain Performance Analysis

Lyapunov Function

Storage Function

Future Works

Visual Feedback System



Fundamental Representation of


Relative Rigid Body Motion

Conclusions

b
wo
b
wc
g
b
co
V
V
V
co




)
(
1
Ad

( is the difference between and .)

b
co
V
b
wc
V
b
wo
V


Nonlinear Observer in Visual Feedback System



Passivity of Visual Feedback System

Fig. 3: Visual Feedback System

co
g
w

o

c

h

wc
g


Dynamic Visual Feedback Control
(with manipulator dynamics)



Uncertainty of the camera coordinate frame
(one of calibration problems)

Energy Function

wo
g
Our proposed methods have the same strategy for both camera configurations.

Systems & Control Laboratory

Kanazawa University

Kanazawa University

18

Appendix

Appendix

Systems & Control Laboratory

Kanazawa University

Kanazawa University

19

Application of Visual Feedback System

Fig. 1: Some Examples of

Visual Feedback System



Swing Automation for Dragline



Autonomous Injection of Biological Cells



Automatic Laparoscope (Surgical Robot)
etc.

Introduction

Control

Vision

Robotics

Research Field of Visual Feedback System

Control will be more important for intelligent

machines as future applications.

Fig. 2: Research Field of VFS

(
Machines
+

Visual Information
)
x

Control



Visual Feedback Control with Fixed
-
camera



Passivity
-
based Control

In this research

Visual Feedback Control

Systems & Control Laboratory

Kanazawa University

Kanazawa University

20

1. Introduction

2. Objective of VFC and Fundamental Representation

3. Nonlinear Observer and Estimation Error System

4. Control Error System

5. Passivity
-
based Control of Visual Feedback System

Outline

6. Conclusions






Systems & Control Laboratory

Kanazawa University

Kanazawa University

21

4
4
ˆ
1
0











R
ab
ab
p
e
g
ab


3
R

ab
p
)
3
(
ˆ
SO
e
ab



wo
wc
co
g
g
g
1






















1
0
1
0
ˆ
ˆ
ˆ
wo
wc
p
e
p
e
e
wo
wc
wc






)
(
co
wc
wo
g
g
g














1
0
)
(
ˆ
ˆ
ˆ
wc
wo
p
p
e
e
e
wc
wo
wc






Position and Orientation

Homogeneous Representation

Fig. a1: Visual Feedback System

Relative Rigid Motion

(a1)

Homogeneous Representation

)
,
(
ˆ
wc
e
p
g
wc
wc



Camera Motion

)
,
(
ˆ
wo
e
p
g
wo
wo



)
,
(
ˆ
co
e
p
g
co
co



Target Object Motion

Relative Rigid Motion

wc
g
wo
g
co
g
w

o

c

3
R

ab

R

ab

Direction of Rotation:

Angle of Rotation:

Rodrigues’ formula

h

Systems & Control Laboratory

Kanazawa University

Kanazawa University

22








wc
wc
b
wc
v
V








wo
wo
b
wo
v
V

wc
wc
b
wc
g
g
V

1
ˆ


wo
wo
b
wo
g
g
V

1
ˆ


4
4
1
:
0
0
ˆ
ˆ











R
ab
ab
ab
ab
b
ab
g
g
v
V


(Ref.: R. Murray
et al.
,
A Mathematical Introduction to Robotic Manipulation
,

1994.)

6
R








ab
ab
b
ab
v
V

)
(
ˆ
1
1
1
1
wo
wc
wo
wc
co
co
co
b
co
g
g
g
g
g
g
g
V










))
1
a
(
(
1
wo
wc
co
g
g
g



)
0
,
(
1
1
1






g
g
g
g
I
gg



)
(
1
1
1
1
1
wo
wo
wo
wc
wo
wc
wc
wc
co
g
g
g
g
g
g
g
g
g










co
g

co
g

b
wo
V
ˆ

Body Velocity

Body Velocity by Homo. Rep.

Body Velocity of Camera Motion

Body Velocity of Target Object Motion

Body Velocity of Relative Rigid Body Motion by Homo. Rep.

: Translation

: Orientation

(a2)

(a3)

(a4)

(a5)

Fundamental Representation of Relative Rigid Body Motion

b
wc
V
ˆ

Systems & Control Laboratory

Kanazawa University

Kanazawa University

23

b
wo
co
b
wc
co
b
wo
co
co
b
wc
co
b
co
V
g
V
g
V
g
g
V
g
V
ˆ
ˆ
)
ˆ
ˆ
(
ˆ
1
1








b
wo
b
wc
g
b
co
V
V
V
co




)
(
1
Ad
(by Adjoint Transformation)

V
V
g
)
(
Ad
'

1
ˆ
'
ˆ


g
V
g
V















ˆ
ˆ
ˆ
)
(
0
ˆ
Ad
e
e
p
e
g
(Adjoint Transformation)

(Ref.: R. Murray
et al.
,
A Mathematical Introduction to Robotic Manipulation
,

1994.)

RRBM

b
wc
V
b
wo
V
)
,
(
ˆ
co
e
p
g
co
co



Body Velocity of Relative Rigid Body Motion

(by Homo. Rep.)

Fig. a2: Block Diagram of RRBM

(a6)

(1)

(a7)

Fundamental Representation of Relative Rigid Body Motion continued

(Fundamental Representation of Relative Rigid Body Motion

剒䉍


Systems & Control Laboratory

Kanazawa University

Kanazawa University

24

Estimation Error System

(10)

(a8)

b
wo
e
g
b
ee
V
u
V
ee




)
(
1
Ad
b
wo
ee
e
ee
b
ee
V
g
u
g
V
ˆ
ˆ
ˆ
1




Estimation Error System

( Detail of Derivation of Estimation Error System )

)
(
ˆ
1
1
1
1
co
co
co
co
ee
ee
ee
b
ee
g
g
g
g
g
g
g
V










)
(
1
1
1
1
1
co
co
co
co
co
co
co
co
ee
g
g
g
g
g
g
g
g
g










)
(
1
co
co
ee
g
g
g



ee
g

ee
g

b
co
V
ˆ

b
co
V
ˆ

)
0
,
(
1
1
1






g
g
g
g
I
gg



b
co
ee
b
co
ee
b
co
ee
ee
b
co
ee
V
g
V
g
V
g
g
V
g
ˆ
ˆ
)
ˆ
ˆ
(
1
1








))
2
(
),
6
(
(

b
wo
ee
e
ee
b
ee
V
g
u
g
V
ˆ
ˆ
ˆ
1





b
wo
ee
e
ee
V
g
u
g
ˆ
ˆ
1




(This system is obtained from (a8) by the property of Adjoint Transformation (a7).)

Systems & Control Laboratory

Kanazawa University

Kanazawa University

25

Image Jacobian

e
e
g
J
f
f
)
(


Relation between Estimation Error

and Image Information

(9)
















)
(
)
(
)
(
)
(
ci
ci
ci
ci
ci
ci
p
p
ci
i
p
p
ci
i
p
p
ci
i
ci
i
i
z
f
y
f
x
f
p
f
f
)
(
)
(
)
(
ci
ci
ci
ci
ci
ci
z
z
y
y
x
x



Taylor Expansion with First Order Approximation

)
(
)
(
1
0
)
(
0
1
2
ci
ci
ci
ci
ci
i
ci
ci
ci
i
ci
ci
ci
i
i
z
z
y
x
z
y
y
z
x
x
z
f





















































ci
ci
ci
ci
ci
ci
i
z
z
y
y
x
x
f
ci
z

2
ci
ci
z
x


0
0
ci
z

2
ci
ci
z
y


ci
ci
p
p


Systems & Control Laboratory

Kanazawa University

Kanazawa University

26

Image Jacobian continued





















1
1
0
ˆ
ˆ
oi
oi
oi
ci
ci
p
p
p
e
e
p
g
p
g
p
p






































1
1
0
)
(
sk
1
1
0
)
(
)
(
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
oi
ee
oi
p
p
e
e
e
p
p
p
e
e
I
e
e
e
ee
















))
(
),
(
sk
,
(
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
p
p
e
p
e
I
e
e
e
e
ee
ee
ee
ee




















)
(
2
1
:
)
(
sk
ˆ
ˆ
ˆ









e
e
e

























)
(
sk
ˆ
)
(
sk
ˆ
ˆ
ˆ
ˆ
ˆ
ˆ
ee
ee
e
p
p
I
e
p
e
e
p
e
p
p
ee
oi
ee
oi
ci
ci










Representation by 4 dimension

Representation by 3 dimension

)
)
(
sk
:
)
(
(
ˆ
ˆ


ee
ee
e
e
e
R









e
oi
R
ee
oi
e
p
I
e
e
e
p
p
I
e
ee
ˆ
)
(
ˆ
ˆ
ˆ
ˆ
















Systems & Control Laboratory

Kanazawa University

Kanazawa University

27

Image Jacobian continued



e
oi
ci
ci
e
p
I
e
p
p
ˆ
ˆ





)
(
ci
ci
i
i
p
p
f
f













ci
z

2
ci
ci
z
x


0
0
ci
z

2
ci
ci
z
y


Relation between Estimation Error and Image Information



e
i
e
oi
i
i
e
g
J
e
p
I
e
f
f
)
(
ˆ
ˆ
















ci
z

2
ci
ci
z
x


0
0
ci
z

2
ci
ci
z
y


e
e
g
J
f
f
)
(



(9)












)
(
)
(
:
)
(
1
g
J
g
J
g
J
m















m
m
i
i
f
f
f
f
f
f

1
1
:


oi
i
p
I
e
g
J
ˆ
:
)
(
ˆ














ci
z

2
ci
ci
z
x


0
0
ci
z

2
ci
ci
z
y


Systems & Control Laboratory

Kanazawa University

Kanazawa University

28

Object Frame

World Frame

Camera Frame

Hand Frame

Fig. 13: Experimental Testbed for Dynamic Visual Feedback Control

Experimental Testbed on a 2DOF Manipulator

I
e
d



ˆ
T
d
p
]
81
.
0
0
0
[


Systems & Control Laboratory

Kanazawa University

Kanazawa University

29

Experiment for Stability Analysis

Fig. 14: Initial Condition

6
50
},
100
,
50
,
50
,
50
,
100
,
100
{
diag
},
15
,
15
{
diag
I
K
K
K
e
c




6
6
,
2
.
0
I
W
I
W
e
c


Field of View

Target Object

2DOF Manipulator

Gains

Fig. 15: Trajectory of Manipulator

Systems & Control Laboratory

Kanazawa University

Kanazawa University

30

Experimental Results

Experimental Results (Stability Analysis)

Fig.16: Control Errors

Fig. 17: Estimation Errors

Fig. 18: Joint Velocity Errors

Fig. 19: Euclid Norm of States

Systems & Control Laboratory

Kanazawa University

Kanazawa University

31

Target Motion in Experiment

The target moves on the
xy

plane for 9.6 seconds.

)
4
0
(


t
)
6
.
9
4
(


t
Fig. 20(a): Straight Motion

Fig.20(b): Figure 8 Motion

L
2
-
gain Performance Analysis in Experiment

Systems & Control Laboratory

Kanazawa University

Kanazawa University

32

Fig. 21: Norm of z

Experimental Results for
L
2
-
gain Performance Analysis

Experimental Results

}
100
,
50
,
50
,
50
,
100
,
100
{
diag

c
K
Gain A

269
.
0


Gain B

225
.
0


}
10
,
10
{
diag
,
15
6



K
I
K
e
}
240
,
120
,
120
,
120
,
240
,
240
{
diag

c
K
}
10
,
10
{
diag
,
30
6



K
I
K
e
6
6
,
1
.
0
I
W
I
W
e
c


6
6
,
1
.
0
I
W
I
W
e
c



represents a disturbance

attenuation level.


Gain A

Gain B