Passivity-based Control and Estimation of Visual Feedback Systems with a Fixed Camera University of Karlsruhe September 30th, 2004 Masayuki Fujita Department of Electrical and Electronic Engineering Kanazawa University, Japan

Introduction Application of Visual Feedback System Camera Image Robot Target Object World Frame Fixed-camera Configuration Camera Swing Automation for Dragline Autonomous Injection of Biological Cells etc. Robot Image Eye-in-Hand Configuration Automatic Laparoscope (Surgical Robot) etc. Target Object World Frame Fig. 1(a): Fixed-camera Fig. 1(a): Eye-in-Hand In the previous work, almost all the proposed methods depend on the camera configuration. 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. Our proposed methods have the same strategy for both camera configurations. Fig. 2: Some Examples of Visual Feedback System

Objective of Visual Feedback Control Position and Orientation Known Information Composition Rule Direction of Rotation: Angle of Rotation: Rodrigues’ formula Unknown Information ( Target motion is unknown.) Homogeneous Representation Fig. 3: Visual Feedback System 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

Fundamental Representation Fig. 3: Visual Feedback System Fundamental Representation of Relative Rigid Body Motion (1) Body Velocity of Camera Body Velocity of Target Object : Translation : Orientation is the difference between and . not measurable Relative Rigid Body Motion in Fixed Camera Configuration : OMFC OMFC (Object Motion from Camera) (2) Fig. 4: Block Diagram of OMFC ( Camera is static. )

Camera Model and Image Information World Frame Camera Frame Image Plane : Focal Length Relative Feature Points (3) Perspective Projection (4) Target Object ( depends on .) Fig. 5: Pinhole Camera Image Information (m points) measurable not measurable OMFC Camera (5) Fig. 6: Block Diagram of OMFC with Camera Image information f includes the relative rigid body motion .

Nonlinear Observer in VFS Fig. 3: Visual Feedback System Estimated OMFC Estimated Body Velocity (6) (2) Model of Estimated Relative Rigid Body Motion：EsOMFC estimated : Input for Estimation Error Estimated Image Information EsOMFC Camera Model (7) (8) Fig. 7: Block Diagram of Estimated OMFC

Nonlinear Observer in VFS continued Estimation Error (Error between Estimated State and Actual One) estimated (Vector Form) Fig. 3: Visual Feedback System measurable not measurable OMFC Camera Relation between Estimation Error and Image Information + (9) Estimation Error System (10) EsOMFC Camera Model estimated Fig. 8: Block Diagram of OMFC and Estimated OMFC

Control Error System Desired RRBM Control Error (Error between Estimated State and Desired One) Fig. 3: Visual Feedback System (Vector Form) Estimated Information Relative Rigid Body Motion from to by Composition Rule (HMFC: Hand Motion from Camera) (11) HMFC + Control Error System (This is dual to the estimation error system.) (12) Fig. 9: Block Diagram of HMFC and Reference

Visual Feedback System Visual Feedback System with Fixed Camera Configuration (13) OMFC Camera Estimation Error System + Control Error System HMFC + EsOMFC Camera Model Fig. 10: Block Diagram of Control and Estimation Error Systems

Visual Feedback System Visual Feedback System with Fixed Camera Configuration (13) OMFC Camera Controller + + HMFC + EsOMFC Camera Model Fig. 11: Block Diagram of Visual Feedback System

Property of Visual Feedback System Lemma 1 If the target is static , then the visual feedback system (13) satisfies (14) where is a positive scalar. OMFC Camera + + Camera Model EsOMFC + HMFC Controller Fig. 11: Block Diagram of Visual Feedback System

Property of Visual Feedback System continued Energy Function (15) Error Function of Rotation Matrix (Proof) Differentiating the energy function (15) with respect to time along the trajectories of the visual feedback system yields skew-symmetric matrices Integrating both sides from 0 to T, we can obtain (14) (Q.E.D.) Passivity Property of Manipulator Dynamics is a skew-symmetric matrix

Stability Analysis Passivity-based Visual Feedback Control Law Gain (16) Gain Theorem 1 If , then the equilibrium point for the closed -loop system (14) and (16) is asymptotically stable. OMFC Camera + + Camera Model EsOMFC + HMFC Controller Fig. 11: Block Diagram of Visual Feedback System

L2-gain Performance Analysis Tracking Problem 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 OMFC Camera + Camera Model EsOMFC + + HMFC Fig. 12: Generalized Plant of Visual Feedback System

L2-gain Performance Analysis Tracking Problem 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 Theorem 2 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 . represents a disturbance attenuation level of the visual feedback system. Other problems can be considered by constructing the adequate generalized plant.

Experimental Testbed on 2DOF Manipulator

Conclusions Visual Feedback System Fundamental Representation of Relative Rigid Body Motion ( is the difference between and .) Nonlinear Observer in Visual Feedback System Fig. 3: Visual Feedback System Passivity of Visual Feedback System Energy Function Stability Analysis Lyapunov Function L2-gain Performance Analysis Storage Function Our proposed methods have the same strategy for both camera configurations. Future Works Dynamic Visual Feedback Control (with manipulator dynamics) Uncertainty of the camera coordinate frame (one of calibration problems)

Appendix Appendix

(Machines + Visual Information) x Control Introduction Application of Visual Feedback System Swing Automation for Dragline Autonomous Injection of Biological Cells Automatic Laparoscope (Surgical Robot) etc. Research Field of Visual Feedback System Control Vision Robotics Fig. 1: Some Examples of Visual Feedback System Visual Feedback Control (Machines + Visual Information) x Control Control will be more important for intelligent machines as future applications. In this research Fig. 2: Research Field of VFS Visual Feedback Control with Fixed-camera Passivity-based Control

Outline 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 6. Conclusions

Homogeneous Representation Position and Orientation Direction of Rotation: Angle of Rotation: Rodrigues’ formula Homogeneous Representation Fig. a1: Visual Feedback System Relative Rigid Motion Camera Motion (a1) Target Object Motion Relative Rigid Motion

Fundamental Representation of Relative Rigid Body Motion Body Velocity Body Velocity by Homo. Rep. : Translation (a2) (a3) : Orientation (Ref.: R. Murray et al., A Mathematical Introduction to Robotic Manipulation, 1994.) Body Velocity of Camera Motion Body Velocity of Target Object Motion (a4) (a5) Body Velocity of Relative Rigid Body Motion by Homo. Rep.

Fundamental Representation of Relative Rigid Body Motion continued Body Velocity of Relative Rigid Body Motion (a6) (by Homo. Rep.) (1) (by Adjoint Transformation) (Fundamental Representation of Relative Rigid Body Motion：RRBM） (a7) (Adjoint Transformation) (Ref.: R. Murray et al., A Mathematical Introduction to Robotic Manipulation, 1994.) RRBM Fig. a2: Block Diagram of RRBM

Estimation Error System (10) (a8) (This system is obtained from (a8) by the property of Adjoint Transformation (a7).) ( Detail of Derivation of Estimation Error System )

Image Jacobian Relation between Estimation Error and Image Information (9) Taylor Expansion with First Order Approximation

Image Jacobian continued Representation by 4 dimension Representation by 3 dimension

Image Jacobian continued Relation between Estimation Error and Image Information (9)

Experimental Testbed on a 2DOF Manipulator Object Frame World Frame Camera Frame Hand Frame The manipulator use in the study, known as SICE-DD arm. We define the four coordinates, world frame, hand frame ,camera frame, and object frame. Let the target object have four feature points. The objective of the visual feedback control is to bring the actual relative rigid body motion g_ho to a given reference g_d. In the study, we set a reference of position and rotation as follows. Fig. 13: Experimental Testbed for Dynamic Visual Feedback Control

Experiment for Stability Analysis Fig. 15: Trajectory of Manipulator Field of View Target Object 2DOF Manipulator We show the Experimental Results for Stability Analysis. Fig. 5 present initial condition. Green square is the field of view of camera. Red square is static target object. And blue line is 2 degrees of freedom manipulator. Fig. 6 show the Trajectory of the Manipulator. You can confirm that the manipulator moved over the target object. Fig. 14: Initial Condition Gains

Experimental Results (Stability Analysis) Fig.16: Control Errors Fig. 17: Estimation Errors This is Experimental Results for Stability Analysis. Fig. 7, and Fig. 8 present the control error vectors e_c and the estimation error vectors e_e, the top graph, the middle one and the bottom one show the error of the translation of the x axis, the error of the translation of the y axis and the error of the rotation of the z axis, respectively. Fig. 9 present the joint velocity error vectors xi, the top graph and the bottom one show the error of 1st axis and 2nd axis. Fig. 10 present the euclid norm of states. Every graph tend to zero. So, it can be concluded that the equilibrium point is asymptotically stable if the target object is static. Fig. 18: Joint Velocity Errors Fig. 19: Euclid Norm of States

L2-gain Performance Analysis in Experiment Target Motion in Experiment The target moves on the xy plane for 9.6 seconds. We show an Experimental Result for L2-gain performance analysis. The target moves on the xy plane for 9.6 seconds. During the first 4 seconds, The object moves along a straight line and during the rest 5.6 seconds, the object moves along a Figure 8 motion. Fig. 20(a): Straight Motion Fig.20(b): Figure 8 Motion

Experimental Results for L2-gain Performance Analysis Gain A Gain A Gain B Gain B Here, we show the Experimental Results for L2-gain performance analysis. Fig. 12 shows the norm of controlled output z. The top graph and the bottom one shows the norm of z in the case of gamma equals 0 point 2 6 9 and 0 point 2 2 5 , respectively. From these two cases, gamma represents a disturbance attenuation level in the case of the moving target object. represents a disturbance attenuation level. Fig. 21: Norm of z