Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera
description
Transcript of Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera
1
Systems & Control LaboratoryKanazawa University
Kanazawa University
Passivity-based Control and Estimation of
Visual Feedback Systems with a Fixed Camera
University of KarlsruheSeptember 30th, 2004
Masayuki Fujita
Department of Electrical and Electronic EngineeringKanazawa University, Japan
2Systems & Control LaboratoryKanazawa University
Kanazawa University
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
WorldFrame
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.
3Systems & Control LaboratoryKanazawa University
Kanazawa University
44ˆ
10
Rab
ab
peg
ab
3Rabp
)3(ˆ
SOe ab
Position and Orientation
Homogeneous Representation
Fig. 3: Visual Feedback System
Objective of Visual Feedback Control
hog
wo
c3Rab
RabDirection 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 dghog
hocowo ggg ,,Unknown Information
( Target motion is unknown.)
wog
hogcog
chwhwc ggg ,,
Known Information
)( 1whwcch ggg
Composition Rule
whg
chg
wcg
4Systems & Control LaboratoryKanazawa University
Kanazawa University
Fundamental Representation
Fig. 3: Visual Feedback System
wcg
wog
cog
wo
c
h
bwo
bwcg
bco VVV
co )( 1Ad
Fundamental Representation of Relative Rigid Body Motion
Body Velocity of Camera:bwcV
:bwoV Body Velocity of Target Object
ˆ
ˆˆ
)(0
ˆAd
e
epeg
6R
ab
abbab
vV
: Translation
: Orientation
is the difference between and .bcoV b
wcV bwoV
Relative Rigid Body Motion in Fixed Camera Configuration : OMFC
bwo
bco VV
( Camera is static. ) 0bwcV
bwoV
cognot measurable
OMFC
Fig. 4: Block Diagram of OMFC
(Object Motion from Camera)
(1)
(2)
5Systems & Control LaboratoryKanazawa University
Kanazawa University
Target Object
cipp
oip
if
wo
c
World Frame
Camera Frame
Image Plane
: Focal Length
Tcicici zyx :
Perspective Projection
Relative Feature Points
ci
ci
cii y
x
zf
Fig. 5: Pinhole Camera
Image Information (m points)
mf
f
f 1
Image information f includes the relative rigid body motion .
Fig. 6: Block Diagram of OMFC with Camera
),(ˆ
coepg coco
Camera Model and Image Information
cip
)(ˆ
coegco copoip oip
( depends on .)if cip
fmeasurable
bwoV
cognot measurable
OMFC Camera
(3)
(4)
(5)
6Systems & Control LaboratoryKanazawa University
Kanazawa University
eb
co uV
Model of Estimated Relative Rigid Body Motion : EsOMFC
),(ˆ
coepg coco
oicoci pgp
Estimated OMFC
eu : 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
wcg
wog
cog
wo
c
h
bwo
bco VV
estimated
fcogEsOMFC
CameraModel
eu
(6) (2)
ci
ci
cii y
x
zf
:
(7)
(8)
7Systems & Control LaboratoryKanazawa University
Kanazawa University
Fig. 8: Block Diagram of OMFC and Estimated OMFC
Nonlinear Observer in VFS continued
),(ˆ
eeepg eeee cocoee ggg 1:
Estimation Error
)(: ˆ
eeee
pe
R
eee (Vector Form)
eegJff )(
Relation between Estimation Error and Image Information f
measurableb
woV
cognot measurable
OMFC Camera
+ee†J
eu fcogEsOMFC
CameraModel
estimated estimated
(Error between Estimated State and Actual One)
Fig. 3: Visual Feedback System
wcg
wog
cog
wo
ch
estimated
(9)
Estimation Error System
bwoeg
bee VuV
ee )( 1Ad (10)
8Systems & Control LaboratoryKanazawa University
Kanazawa University
),(ˆ
depg dd
)(: ˆ
ecee
pe
R
ecc
),(ˆ
ecepg ecec hodec ggg 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
hogcog
wo
ch
chg
hoco gg ,Estimated Information
):( 1cochho ggg
by Composition Rule
eb
whg
bho uVV
ho )( 1Ad
Relative Rigid Body Motion from toh o
(HMFC: Hand Motion from Camera)
(11)
bdge
bwhg
bec VuVV
echo )()( 11 AdAd Control Error System
(This is dual to the estimation error system.)
(12)
eub
whV hogdg ce
HMFC+
9Systems & Control LaboratoryKanazawa University
Kanazawa University
Control Error System
Estimation Error System
Visual Feedback System with Fixed Camera Configuration
Visual Feedback System
bwoceb
ee
bec V
Iu
V
V
0)( 1Ad hog
)( 1Ad eeg
I
0
e
bdg
bwh
ceu
VVu d )(Ad
:
e
c
e
ee :
Fig. 10: Block Diagram of Control and Estimation Error Systems
bwoV
fcogOMFC Camera
eu
fcogEsOMFC
CameraModel
+†J
ee
(13)
dgce
bwhV
hogHMFC
+
10Systems & Control LaboratoryKanazawa University
Kanazawa University
Visual Feedback System with Fixed Camera Configuration
Visual Feedback System
bwoceb
ee
bec V
Iu
V
V
0)( 1Ad hog
)( 1Ad eeg
I
0
Fig. 11: Block Diagram of Visual Feedback System
Controller
ceu+
bdg V
d )(Ad
bwoV
fcogOMFC Camera
eu
fcogEsOMFC
CameraModel
+†J
ee
(13)
e
bdg
bwh
ceu
VVu d )(Ad
:
e
c
e
ee :
dgce
bwhV
hogHMFC
+
11Systems & Control LaboratoryKanazawa University
Kanazawa University
Property of Visual Feedback System
Fig. 11: Block Diagram of Visual Feedback System
Controller
ceu+
bdg V
d )(Ad fcog
eu
fcog
CameraModel
dgce
bwhV hog
+†J
ee
Lemma 1 If the target is static , then the visual feedback system (13) satisfies
cece
T Tce du 0
)0( bwoV
ce,where is a positive scalar.
INeN
ec
d
e
T
gcecece
)(
)(
ˆ
1
Ad
0Ad:,:
OMFC Camera
EsOMFC
HMFC
ceceN
(14)
+
12Systems & Control LaboratoryKanazawa University
Kanazawa University
(Proof) Differentiating the energy function (15) with respect to time along
the trajectories of the visual feedback system yields
),(2)( qqCqM
eeweTeeecwc
Tecce
egT ppppuI
eV ecd ˆˆ0
AdAd)()( ˆ1
ceTceu
is a skew-symmetric matrix
skew-symmetric matrices
Integrating both sides from 0 to T, we can obtain
cece
T Tce VVTVdu :)0()0()(
0(14)
Passivity Property of Manipulator Dynamics
Tce
(Q.E.D.)
Property of Visual Feedback System continued
)(2
1)(
2
1 ˆ2ˆ2eeec epepV eeec
Energy Function(15)
)(tr2
1:)(
ˆˆ eIe Error Function of Rotation Matrix
13Systems & Control LaboratoryKanazawa University
Kanazawa University
If , then the equilibrium point for the closed
-loop system (14) and (16) is asymptotically stable.
0e
Passivity-based Visual Feedback Control Law
Stability Analysis
Theorem 1
)( cecececece KeNKu 0:
ceK cK
eK00
Gain
Fig. 11: Block Diagram of Visual Feedback System
Controller
+
bdg V
d )(Ad fcog
eu
fcog
CameraModel
dgce
bwhV hog
+†J
eeOMFC Camera
EsOMFC
HMFC
ceN ceuce ceK
(16)
0bwoV
+
14Systems & Control LaboratoryKanazawa University
Kanazawa University
Tracking Problem
L2-gain Performance Analysis
Based on the dissipative systems theory, we consider L2-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
ceu
bwoV
+
fcog
eu
fcog
bwhV hog
+
OMFC Camera
EsOMFC
HMFC
,Ad )(b
dg Vd dg
ceK ceN
†J e
+
z
CameraModel
15Systems & Control LaboratoryKanazawa University
Kanazawa University
Tracking Problem
L2-gain Performance Analysis
Based on the dissipative systems theory, we consider L2-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 L2-gain .
cK eK
Theorem 2
},0{diag:,2
1
2
1:
2IWIWNKNP cece
Tce
• represents a disturbance attenuation level of the visual feedback system.• Other problems can be considered by constructing the adequate generalized plant.
16Systems & Control LaboratoryKanazawa University
Kanazawa University
Experimental Testbed on 2DOF Manipulator
17Systems & Control LaboratoryKanazawa University
Kanazawa University
• Stability Analysis
• L2-gain Performance Analysis
Lyapunov Function
Storage Function
Future Works
Visual Feedback System• Fundamental Representation of Relative Rigid Body Motion
Conclusions
bwo
bwcg
bco VVV
co )( 1Ad
( is the difference between and .)bcoV b
wcV bwoV
• Nonlinear Observer in Visual Feedback System
• Passivity of Visual Feedback System
Fig. 3: Visual Feedback System
cog
wo
ch
wcg
• Dynamic Visual Feedback Control (with manipulator dynamics)
• Uncertainty of the camera coordinate frame (one of calibration problems)
Energy Function
wog
Our proposed methods have the same strategy for both camera configurations.
18Systems & Control LaboratoryKanazawa University
Kanazawa University
Appendix
Appendix
19Systems & Control LaboratoryKanazawa University
Kanazawa University
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
20Systems & Control LaboratoryKanazawa University
Kanazawa University
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
21Systems & Control LaboratoryKanazawa University
Kanazawa University
44ˆ
10
Rab
ab
peg
ab
3Rabp
)3(ˆ
SOe ab
wowcco ggg 1
1010
ˆˆˆ
wowc pepee wowcwc
)( cowcwo ggg
10
)(ˆˆˆ
wcwo ppeee wcwowc
Position and Orientation
Homogeneous Representation
Fig. a1: Visual Feedback System
Relative Rigid Motion
(a1)
Homogeneous Representation
),(ˆ
wcepg wcwc
Camera Motion
),(ˆ
woepg wowo
),(ˆ
coepg coco
Target Object Motion
Relative Rigid Motion
wcg
wog
cog
wo
c3Rab
RabDirection of Rotation:
Angle of Rotation:
Rodrigues’ formula
h
22Systems & Control LaboratoryKanazawa University
Kanazawa University
wc
wcbwc
vV
wo
wobwo
vV
wcwcb
wc ggV 1ˆ wowob
wo ggV 1ˆ
441:00
ˆˆ
Rabab
ababbab gg
vV
(Ref.: R. Murray et al., A Mathematical Introduction to Robotic Manipulation, 1994.)
6R
ab
abbab
vV
)(ˆ 1111wowcwowccococo
bco gggggggV ))1a(( 1
wowcco ggg
)0,( 111 ggggIgg )( 11111wowowowcwowcwcwcco ggggggggg
cog cog bwoV̂
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
bwcV̂
23Systems & Control LaboratoryKanazawa University
Kanazawa University
bwoco
bwcco
bwococo
bwcco
bco VgVgVggVgV ˆˆ)ˆˆ(ˆ 11
bwo
bwcg
bco VVV
co )( 1Ad (by Adjoint Transformation)
VV g )(Ad'1ˆ'ˆ gVgV
ˆ
ˆˆ
)(0
ˆAd
e
epeg
(Adjoint Transformation)
(Ref.: R. Murray et al., A Mathematical Introduction to Robotic Manipulation, 1994.)
RRBM
bwcV
bwoV
),(ˆ
coepg coco
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: RRBM)
24Systems & Control LaboratoryKanazawa University
Kanazawa University
Estimation Error System
(10) (a8)b
woeg
bee VuV
ee )( 1Ad b
woeeeeeb
ee VgugV ˆˆˆ 1
Estimation Error System
( Detail of Derivation of Estimation Error System )
)(ˆ 1111cocococoeeeeee
bee gggggggV
)( 11111cocococococococoee ggggggggg
)( 1cocoee ggg
eeg eeg bcoV̂b
coV̂
)0,( 111 ggggIgg
bcoee
bcoee
bcoeeee
bcoee VgVgVggVg ˆˆ)ˆˆ( 11
))2(),6((
bwoeeeee
bee VgugV ˆˆˆ 1
bwoeeeee Vgug ˆˆ1
(This system is obtained from (a8) by the property of Adjoint Transformation (a7).)
25Systems & Control LaboratoryKanazawa University
Kanazawa University
Image Jacobian
eegJff )(Relation between Estimation Error and Image Information (9)
)()()(
)(cicicicicici ppci
i
ppci
i
ppci
iciii z
f
y
f
x
fpff )()()( cicicicicici zzyyxx
Taylor Expansion with First Order Approximation
)()(1
0)(
0
12 cici
ci
ci
ci
icici
ci
icici
ci
ii zz
y
x
zyy
zxx
zf
cici
cici
cici
i
zz
yy
xx
f
ciz
2ci
ci
z
x
0
0ciz
2ci
ci
z
y
cici pp
26Systems & Control LaboratoryKanazawa University
Kanazawa University
Image Jacobian continued
110
ˆˆoi
oioicici
pppeepgpgpp
110
)(sk110
)()(ˆˆˆˆˆˆˆˆ
oieeoi ppeeepppeeIeee ee
))(),(sk,(ˆˆˆˆˆˆ
ppepeIeeee eeeeeeee
)(2
1:)(sk
ˆˆˆ eee
)(skˆ
)(skˆ ˆ
ˆˆ
ˆˆ
ee
ee
e
ppIe
p
eepepp ee
oi
ee
oicici
Representation by 4 dimension
Representation by 3 dimension
))(sk:)((ˆˆ eeee eeeR
eoiR
eeoi epIe
ee
ppIe
eeˆ
)(ˆ
ˆ
ˆ
ˆ
27Systems & Control LaboratoryKanazawa University
Kanazawa University
Image Jacobian continued
eoicici epIepp ˆˆ
)( ciciii ppff
ciz
2ci
ci
z
x
0
0ciz
2ci
ci
z
y
Relation between Estimation Error and Image Information
eieoiii egJepIeff )(ˆˆ
ciz
2ci
ci
z
x
0
0ciz
2ci
ci
z
y
eegJff )( (9)
)(
)(
:)(1
gJ
gJ
gJ
m
mm
ii
ff
ff
ff 11
:
oii pIegJ ˆ:)(ˆ
ciz
2ci
ci
z
x
0
0ciz
2ci
ci
z
y
28Systems & Control LaboratoryKanazawa University
Kanazawa University
Object Frame
World FrameCamera Frame
Hand Frame
Fig. 13: Experimental Testbed for Dynamic Visual Feedback Control
Experimental Testbed on a 2DOF Manipulator
Ie d ̂
Tdp ]81.000[
29Systems & Control LaboratoryKanazawa University
Kanazawa University
Experiment for Stability Analysis
Fig. 14: Initial Condition
650},100,50,50,50,100,100{diag},15,15{diag IKKK ec
66 ,2.0 IWIW ec
Field of View
Target Object
2DOF Manipulator
Gains
Fig. 15: Trajectory of Manipulator
30Systems & Control LaboratoryKanazawa University
Kanazawa University
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
31Systems & Control LaboratoryKanazawa University
Kanazawa University
Target Motion in Experiment
The target moves on the xy plane for 9.6 seconds.
)40( t )6.94( t
Fig. 20(a): Straight Motion Fig.20(b): Figure 8 Motion
L2-gain Performance Analysis in Experiment
32Systems & Control LaboratoryKanazawa University
Kanazawa University
Fig. 21: Norm of z
Experimental Results for L2-gain Performance Analysis
Experimental Results
}100,50,50,50,100,100{diagcK
Gain A 269.0
Gain B 225.0
}10,10{diag,15 6 KIKe
}240,120,120,120,240,240{diagcK
}10,10{diag,30 6 KIKe
66 ,1.0 IWIW ec
66 ,1.0 IWIW ec
represents a disturbance
attenuation level.
Gain A
Gain B