relative rigid body motion motion control · iccas-sice 2009 tutorial mini-course 9:00 – 11:00,...

Fujita LaboratoryTokyo Institute of Technology

Tokyo Institute of Technology

Visual Feedback for Cooperative Motion Control

ICCAS-SICE 2009 Tutorial Mini-Course9:00 – 11:00, August 21st, 2009

Masayuki FujitaDepartment of Mechanical and Control Engineering

Tokyo Institute of Technology, JapanFujita LaboratoryTokyo Institute of Technology


Outline

・ Relative Rigid Body Motion

・ Prologue – Cooperative Control

・ Pose Control with VMO

・ Visual Motion Observer (VMO)

・ Robot Control with VMO

・ Epilogue – Cooperative Control with VMO

2



3

Agent i’s System Dynamics : State: Output: Input

Agent i’s Storage Function

Passivity

Fig.1: Block Diagram of Agenti’s System Dynamics

Passive

Storage FunctionSystem Dynamics : State: Output: Input

Passivity

（1）

（2）（3）

（4）

（7）

（5）（6）

Definition of Passivity



（12）

4

Closed-loop Systems

Networked Passive Systems

Passive

Fig. 2: Networked Passive Systems

Networked Passive Systems (Three Agents)

Fig. 3: Simulation Result

SynchronizationOutput

Control Input: Output Error with Neighbors

Fig. 4: Networked Passive Systems in SE(3)

SE(3)

（8）（9）（10）

（14）

（11）（13）

Lyapunov Function Candidate

Sum of Individual Storage Functions（17）

（18）

（15）（16）

4

ICCAS-SICE 2009 Tutorial Mini-Course August 21, 2009, Fukuoka International Congress Center, Fukuoka, JAPAN



Position (Translation)：

Orientation (Rotation)：

Homogeneous Transformation

5

Fig. 5: Homogeneous Transformation

Homogeneous Transformation

“∧” (wedge) : “∨” (vee) :

: Rotation Axis: Rotation Angle

Exponential Coordinate for Rotation

Fig. 6: Exponential Coordinatefor Rotation

（19）（20）

（23）

（21）（22）



6

Rigid Body Motion in SE(3)

: Position

: Orientation

Fig. 9: Block Diagram of Rigid Body Motion

Rigid BodyMotion

(24)

Pose (Position and Orientation)

(27)Rigid Body Motion

:Velocity of relative to as viewed in the current wedge

vee（25）

（26）

Body Velocity

Fig. 7: Body Velocity

（26）

Fig. 8: Coordinate of Rigid Body in SE(3)



Passivity of Rigid Body Motion

7

The rigid body motion (27) satisfies

where is a positive scalar.

Lemma 1

Passive

（31）

Rigid BodyMotion to

Vector Representation of Pose(Position)

(Orientation)（28）

: Coordinate Transformationof Vector from to

( to )

to Skew-symmetric Component

（29）（30）

Fig. 10: Block Diagram of Passivity of Rigid Body Motion

Fig. 8: Coordinate of Rigid Body in SE(3)

(27)Rigid Body Motion



8

(33)Attitude Synchronization

Consider the n rigid bodies represented by (27). Then a group of rigid bodies is said to achieve attitude synchronization, when all rigid bodies converge to the same orientation between the rigid bodieswhile moving in the same direction.

Attitude Synchronization

Error Function of Rotation Matrix Property :・

・

・


Relative Orientation

（32）

(34)Fig. 11: Attitude Synchronization

j-th rigid body’s orientation as viewed in



Fig. 12: Network Topology

9

Control Input for Attitude Synchronization

Control Input for Attitude Synchronization

(36)

Linear Velocity Input

Angular Velocity Input

Relative Orientation

(35)

Fig. 13: Block Diagram of Attitude Synchronization

i-th Attitude Synchronization Controller

i-th RigidBody Motion

j-th RigidBody Motion

Neighborhood : A set of agents whose information is available to agent i

Fig. 11: Attitude Synchronization



10


Theorem 1Consider the n rigid bodies represented by (27). Then, under the assumptions A1 and A2, the velocity input (35), (36) achieves attitude synchronization in the sense of (33).

There is a directly path connecting any two distinct nodes.

All agents are able to get any agent’s information direct or indirectly.

Strongly Connected

Assumptions(A1)(A2) The graph is strongly connected and fixed

are positive definite

: positive definite

Fig. 12: Network Topology




Outline







11Fujita LaboratoryTokyo Institute of Technology


Relative Pose and Body VelocityPose of Vision Camera

Pose of Object（37）

（38）Pose of Object relative to

（39）

（40）

12

Body Velocity of Vision Camera

Body Velocity of Object（41）

（43）

Body Velocity of Object relative to

（45）

（42）

（46）

（44）

Vision Camera

Vision Camera

: Position

: Orientation

Object Frame World Frame

Vision Camera Frame

Fig. 14: Relative Pose and Body Velocity of Object relative to Vision Camera in SE(3)



13

Relative Rigid Body Motion

Differentiating (40) w.r.t. time（40）Pose of Object relative to Vision Camera:

（47）

=（47）

（48）

=（42） =（44）



Vision Camera Frame



14

vee（49）

Adjoint Transformation

（50）Relative Rigid Body Motion

Body Velocity ofVision Camera

（52）Body Velocityof Object

: Coordinate Transformationfrom to


Fig. 15: Block Diagram of Body Velocity of Object relative to Vision Camera

+−

（48）

Body Velocity of Object relative to

（53）

（51）

Vision Camera



Vision Camera Frame



Vector Representation of Pose

Fig. 16: Block Diagram of Vector Representation of PoseRelative Rigid Body Motion

（40）

Vector Representation of Pose(Position)

(Orientation)（55） to

( to )

to

Vector Representation of Rotation Matrix

MatrixVector

（54）

Skew-symmetric Component



Passivity of Relative Rigid Body Motion

Lemma 2

（56）

where is a positive scalar.

If the object is static , then the relative rigid body motion (52) satisfies

Rotation Matrix

Property :・

・

・

Error Function of

（58）Storage Function

（57）


Skew-symmetric Matrix

Differentiating (58) w.r.t. time yields(Proof)

（59）



(Q.E.D.)

Integrating (60) from 0 to T, we obtain

where is a positive scalar that only depends on the initial states of .

（61）

PassiveRelative RigidBody Motion to

Fig. 17: Block Diagram of Passivity of Relative Rigid Body Motion

Passivity of Relative Rigid Body Motion

（32）Property of Skew-symmetric

（60）

Remark 1

motion (52) satisfies where is aIf the vision camera is static , then the relative rigid body

Passivepositive scalar.Fujita LaboratoryTokyo Institute of Technology


Outline







18



19

Feature Points

Fig. 19：Pinhole Camera Model

: not measurable

World FrameObject Frame World FrameUnknown

Vision CameraFrame

not measurable

Unknown

Relative RigidBody Motion

Fig. 18: Block Diagram of Relative Rigid Body Motion with Vision Camera

Object’s i-th Feature Point

PointFeature

（63）

i-th Feature Point

PointsFeature

（62）



（64）

Image Information (m Points)

（65）

20

Perspective Projection

Perspective Projection (Pinhole Camera)

Fig. 19：Pinhole Camera ModelUnknown

not measurableVision Camera

measurable

PointsFeature

ProjectionPerspective

Unknown

Relative RigidBody Motion

Fig. 18: Block Diagram of Relative Rigid Body Motion with Vision Camera

World FrameObject Frame World Frame

Vision CameraImage Plane

: Focal Length

Frame

PointFeature



Relative Rigid Body Motion (RRBM)

Actual Posenot measurable

measurableImage Information

Luenberger Observer

（64）（65）

（52）

RRBM Camera

not measurable measurable

Vision

Unknown

Fig. 20: Block Diagram of Estimation Error Vector

（66）：Input for Estimation Error

Relative Rigid Body Motion (RRBM) Model

Estimated Image Information

Estimated Pose

（67）（68）

to ControlInput

estimated

ModelCamera

estimated

RRBM Vision

Model21



Estimation Error

Estimation Error Vector

（69）(Error between Estimated State and Actual One)

Estimation Error

(Position)(Orientation)（70）

Image Information Error（71）

(Actual) ImageInformation

Estimated Image Information


estimated

ModelCamera

RRBM Camera


estimated

RRBM

Vision

Vision

Model

Unknown

Relation between the actual image information and the estimated one

: i-th Image Jacobian（72）

22



Estimation Error System


estimated

ModelCamera

RRBM Camera


estimated

RRBM

Vision

Vision

Model

Estimation Error（69）

Estimation Error Vector

（70）Image

Jacobian

（73）Image

Information

Estimation error can be calculatedusing image information !!

PoseInformation

（75）

Estimation Error System（74）

System

estimated

ModelCamera

RRBM Camera


estimated

RRBM

ErrorEstimation

Vision

Vision

Model23



Passivity of Estimation Error System

Lemma 3

（76）where and is a positive scalar.

If the object is static , then the estimation error system (75) satisfies

Passive

Fig. 21: Block Diagram of Passivity of Estimation Error System

System

estimated

ModelCamera

RRBM Camera


estimated

RRBM

ErrorEstimation

Vision

Vision

Model

Passive


(Sketch of Proof)

（78）

Storage Function

（77）

24



Fig. 22: Block Diagram of Visual Motion Observer

Control Law for Visual Motion Observer : Passivity Approach

Visual Motion Observer

（79）Theorem 2If , then the equilibrium point for the closed-loop system (75) and (79) is asymptotic stable.

Lyapunov Function Candidate（77）

（80）System

estimated

ModelCamera

RRBM Camera


estimated

RRBM

ErrorEstimation

Vision

Vision

Model

Fig. 23: Block Diagram of Visual Motion Observer

MotionVisual

Observer




Outline







26



Fig. 25: Block Diagram of Relative Pose Computation

Pose ControlBring the relative poseto the desired pose .

× : not measurable only from f

Unknown Fig. 24:Pose Control ofEye-in-HandSystem

ConstantDesiredPose

Pose Control of Eye-in-Hand System

MotionVisual

Observer

×（81）（69）

to

Pose Computation

( to )to

○（81）

If Attitude Estimation Error

Vector Matrix（82）



Pose Control Error System

（85）


Pose Control Error

Pose Control Error Vector

（83）

（84）

Unknown

Passive

(This is dual to the estimation error system.)

Desired Pose :Constant

Fig. 26: Block Diagram of Pose Control Error System

Pose Computation

toto

Pose ControlError System

to

Fig. 24:Pose Control ofEye-in-HandSystem

MotionVisual

Observer

28



Fig. 27: Block Diagram of Error System for Pose Control with Visual Motion Observer

Error System for Pose Control with Visual Motion Observer


estimated

measurable

Estimation Error System

（86）

State Input Disturbance

Error System for Pose Control with Visual Motion Observer

Pose Control & Estimation


Estimation Error System＋

totoRRBM& Camera

Model

Error Systems



Fig. 27: Block Diagram of Error System for Pose Control with Visual Motion Observer

Pose Control & Estimation Error Systems

Passivity of Error System for Pose Control with Visual Motion Observer

Error SystemPose Control

Error SystemEstimation

Lemma 4If the object is static , then the error system forpose control with the visual motion observer (86) satisfies

（87）

where , and is a positive scalar.

Passive

Passive

Storage Function（88）


(Sketch of Proof)（89）

（90）

30



Fig. 28: Block Diagram of Pose Control with Visual Motion Observer

Pose Control Law

Pose Control Law with Visual Motion Observer : Passivity Approach

（91）

Pose Control Law and Stability Analysis

Error SystemEstimation

Theorem 3If , then the equilibrium point for theclosed-loop system (86) and (91) is asymptotic stable.



Lyapunov Function Candidate（88）（92）




Pose Control Law

Pose Control with Visual Motion Observer

Pose Controller

RRBM& Camera

Model




Motion ControllerPose


Velocity InputVisual

Observer

Pose Computation

toto

Pose ControlError System Pose Controller

Fig. 29: Block Diagram of Pose Controller

32



Outline









ControllerPose


Manipulator Dynamics


MotionVisual

Observer

No Dynamics

Manipulator Dynamics

: Inertia Matrix

: Gravity Vector

: Input Torque: Coriolis Matrix : Joint Angle

（93）

：Skew-symmetric

Fig. 31: 2DOF Manipulator

1q

2q

X

Y

1r 2r

1m

2m 2I

1I

1l

2lExample : Two Degree of Freedom Planar Manipulator

Passivity of Manipulator Dynamics



Fig. 33: Block Diagram of Manipulator

Fig. 32: Block Diagram of Pose Control with Visual Motion Observer & Manipulator

ControllerPose

MotionVisual

Observer

Manipulator Dynamics（94）：Disturbance InputBody Velocity of Vision Camera

：Body Manipulator（95）

ManipulatorDynamics

Joint Velocity Error

Jacobian

Desired JointVelocity

Joint Velocity Error

Vision Camera Velocityfrom Pose Controller

（97）

（96）

Desired



Fig. 32: Block Diagram of Pose Control with Visual Motion Observer & Manipulator

Fig. 34: Block Diagram of Robot Controller

ControllerRobot

ControllerPose

MotionVisual

Observer

Control

Passivation

Robot ControllerPassivation Control

（98）：Input for Joint Velocity Error

Joint Velocity Error System

（99）

Fig. 35: Block Diagram of Joint Velocity Error System

Control

Passi-Robot Controller

Joint VelocityError System

vation

PassiveIf

Passivation Control



Fig. 36: Block Diagram of Error System for Robot Control with Visual Motion Observer

Error System

Error System for Robot Control with Visual Motion Observer

Error System for Robot Control with Visual Motion Observer

（100）

State Input Disturbance

Error Systems

Pose Control& Estimation

Error SystemJoint Velocity



Fig. 36: Block Diagram of Error System for Robot Control with Visual Motion Observer

Passivity of Error System for Robot Control with Visual Motion Observer

Error System

Error Systems



Passive

Lemma 5If , then the error system for robot controlwith the visual motion observer (100) satisfies

（101）

where ,and is a positive scalar.

Storage Function

（102）

Passive

38



39

Differentiating (102) w.r.t. time yields

Skew-symmetricMatrix

Skew-symmetricMatrix

（103）

(Q.E.D.)

Integrating (103) from 0 to T, we obtain

where is a positive scalar that only depends on the initial states of, and .

Passivity of Error System for Robot Control with Visual Motion Observer

(Proof)

（104）

Joint Velocity Error System(Manipulator Dynamics)

Pose ControlError System

EstimationError System



Fig. 37: Block Diagram of Robot Control with Visual Motion Observer

Robot Control Law with Visual Motion Observer

（105）

：Gain for Joint Velocity Error：Gain for Pose Control Error：Gain for Estimation Error

Robot Control Law

Robot Control Law with Visual Motion Observer : Passivity Approach

Control

Error System

Error Systems



40



Fig. 37: Block Diagram of Robot Control with Visual Motion Observer

Stability Analysis for Robot Control with Visual Motion Observer

Theorem 4If , then the equilibrium point for the closed-loop system (100) and (105) is asymptotic stable.

Lyapunov Function Candidate

The equilibrium point is asymptotically stable.

Fig. 38: Concept ofLyapunov Function

Robot Control Law

Error System

（106）

（107）

Error Systems





Outline







42



Rigid Body Motion in SE(3)

43

(24)

Pose (Position and Orientation)

(27)Passive

Rigid BodyMotion to

Lemma 1:


Review: Attitude Synchronization

Attitude Synchronization(33)

Fig. 38: Coordinates of Multi-rigid Body in SE(3)

Relative Pose（107）

Control Input(35)

(36)

Fig. 10: Block Diagram of Passivity of Rigid Body Motion



44

Pose Synchronization

Consider the n rigid bodies represented by (27). Then a group of rigid bodies is said to achieve pose synchronization, when all rigid bodies converge to the same pose between the rigid bodies.



Relative Pose

(109)

(108)

(77)

Fig. 39: Pose Synchronization

(24)



45

Control Input for Pose Synchronization



(110)Relative Pose

Fig. 40: Block Diagram of Pose Synchronization

i-th Pose Synchronization Controller



Control Input for Pose Control under the Condition:

（91）

Sum of Input for Pose Control with Neighbors

(110)



46


Theorem 4Consider the n rigid bodies represented by (27). Then, under the assumptions A1 and A2, the velocity input (110) achieves pose synchronization in the sense of (108).

Position

Lyapunov Function Candidate（111）

Orientationare uniquely defined by strongly connected graphs.



i-th Pose Synchronization

Controller


j-th Pose Synchronization

Controller


Interaction



47



Visual

Observer


（110）

Visual Motion Observer× : not measurableRelative Pose



Pose Synchronization with Visual Motion Observer



Controller

×

not measurablenot measurable




48



Visual

Observer

Motioni-th Visual

Observeri-th Rigid

Body Motion


Controller

Fig. 42: Block Diagram of Pose Synchronization with Visual Motion Observer

Pose Synchronization with Visual Motion Observeri-th PoseCompu-tation

Estimated Relative PoseEstimation ErrorEstimation Error Vector

（112）

Image Information

RRBM CameraVision



(RRBM)

（113） Fig. 18: Block Diagram of Relative RigidBody Motion with Vision Camera

i-thVision

Camera




49

Motioni-th Visual

Observeri-th Rigid

Body Motion


Controller

i-th PoseCompu-tation


Interaction between Multiple Rigid Bodies

Motioni-th Visual

Observerand Pose Synchronization

i-th Pose Computation

Controller

Fig. 43: Block Diagram of Pose Synchronization with Visual Motion Observerin 2 Rigid Bodies

Interaction

Motionj-th Visual

Observerand Pose Synchronization

j-th Pose Computation

Controller

i-thVision

Camera



Computationi-th Posei-th

Observer

50

Fig. 44: Block Diagram of i-th Multi-visual Motion Observer

Multi-visual Motion Observer

Pose Synchronization Control Law Pose Synchronization Control Law

（115）

VisualMotion

ObserverVisualMotion

Observer



Fig. 38: Coordinates of Multi-rigid Body in SE(3)

（114）


with Visual Motion Observer

PoseCompu-tation

PoseCompu-tation


Controller

Motioni-th Visual

Observeri-th Rigid

Body Motion


Controller

i-th PoseCompu-tation

i-thVision

Camera



Outline







51

relative rigid body motion motion control · iccas-sice 2009 tutorial mini-course 9:00 – 11:00,...

Documents