Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

32
1 Systems & Control Laboratory Kanazawa University Kanazawa University 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 Engin eering Kanazawa University, Japan

description

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. Introduction. Image. Camera. Robot. World Frame. - PowerPoint PPT Presentation

Transcript of Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

Page 1: 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

Page 2: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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.

Page 3: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 4: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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)

Page 5: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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)

Page 6: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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)

Page 7: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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)

Page 8: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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+

Page 9: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

+

Page 10: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

+

Page 11: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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)

+

Page 12: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 13: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

+

Page 14: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 15: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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.

Page 16: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

16Systems & Control LaboratoryKanazawa University

Kanazawa University

Experimental Testbed on 2DOF Manipulator

Page 17: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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.

Page 18: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

18Systems & Control LaboratoryKanazawa University

Kanazawa University

Appendix

Appendix

Page 19: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 20: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 21: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 22: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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̂

Page 23: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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)

Page 24: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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).)

Page 25: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 26: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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ˆ

)(ˆ

ˆ

ˆ

ˆ

Page 27: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 28: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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[

Page 29: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 30: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 31: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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

Page 32: Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera

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