3rd SUMMER EUROPEAN UNIVERSITY Surgical robotics – Montpellier, September 2007 VISUAL SERVOING with applications in medical robotics Florent Nageotte, Michel de Mathelin Louis Pasteur University – Strasbourg LSIIT – Control, Vision & Robotics Research Group UMR CNRS 7005 1 OverviewOverview Part I : Fundamentals of visual servoing Background and definitions Servoing architectures and classification Position-based visual servoing Image-based visual servoing Visual servoing without feature extraction Part II : Medical robotics applications Laparoscopic surgery Internal organ motion tracking 2 European Summer University - Montpellier 2007 OverviewOverview Part I : Fundamentals of visual servoing Background and definitions Servoing architectures and classification Position-based visual servoing Image-based visual servoing Visual servoing without feature extraction Part II : Medical robotics applications Laparoscopic surgery Internal organ motion tracking 3 European Summer University - Montpellier 2007 VisualVisual servoingservoing principleprinciple Control the end-effector of a robot using a vision sensor (2D sensor). E.g : position the effector of a robot w.r.t. an object Similar to registration but the velocity of the robot is generally continuously updated Minimize a task function dependent of the error between the current pose of the robot and the reference pose Creates a virtual link between robot and object Image processing, computer vision and control issues 4 European Summer University - Montpellier 2007 I.1I.1 BackgroundBackground andand definitionsdefinitions A. Coordinates and pose Coordinates of point P with respect to coordinate frame i : i P Position and orientation of frame i with respect to frame j : ⎡T ⎤ ⎫ origin of x ⎪ ⎢T ⎥ translation vector = jT frame i w.r. ⎢ y ⎥ ⎬ i ⎪ to frame j j ⎢Tz ⎥ ⎭ Pose = pi = ⎢α ⎥ ⎫ ⎢ ⎥ rotation ⎪ j ⎢ β ⎥ ⎬ rotation angles ⇒ Ri matrix ⎢ ⎥ ⎪ ⎣ γ ⎦ ⎭ 5 European Summer University - Montpellier 2007 I.1I.1 BackgroundBackground andand definitionsdefinitions B. Coordinate transformations Coordinates of point iP with respect to coordinate frame j : j j j i P = Ti + Ri P Coordinates of vector iV with respect to frame j : j j i V = Ri V Homogeneous transformation from frame i to frame j : j j j i j i j ⎡ Ri Ti ⎤ ⎡ P⎤ j ⎡ P⎤ ⎡ V ⎤ j ⎡ V ⎤ H i = ⎢ ⎥ ⇒ ⎢ ⎥ = H i ⎢ ⎥ ⎢ ⎥ = H i ⎢ ⎥ ⎣ 0 1 ⎦ ⎣ 1 ⎦ ⎣ 1 ⎦ ⎣ 0 ⎦ ⎣ 0 ⎦ j j k H i = H k H i 6 European Summer University - Montpellier 2007 I.1I.1 BackgroundBackground andand definitionsdefinitions C. Velocity of a rigid object Velocity screw of frame i with respect to frame j in frame j coordinates: ⎡ vx ⎤ ⎫ ⎪ j ⎢ v ⎥ translatio nal velocity = jV ⎢ y ⎥ ⎬ ()i ⎪ j ⎢ v ⎥ j z ⎭ ()ri = ⎢ ⎥ ω x ⎫ ⎢ ⎥ ⎪ j j ⎢ω y ⎥ ⎬ rotational velocity = ()Ωi ⎢ ⎥ ⎪ ⎣ω z ⎦ ⎭ Velocity of point P rigidly attached to frame i with respect to frame j expressed in frame j coordinates : j j j j j j j j j i j j j P = ( Ω i )× P + ( Vi ) = ( Ω i )× ( Ri P+ Ti ) + ( Vi ) 7 European Summer University - Montpellier 2007 I.1I.1 BackgroundBackground andand definitionsdefinitions D. Camera projection model Perspective projection : image plane P camera frame Yc p Xc y x Zc oc optical center λ v u λ = focal length c P = coordinates of point P with respect to the camera frame c ⎡ x c ⎤ ⎡ x c ⎤ ⎡ x c ⎤ u + λ k ⎡ x ⎤ ⎢ z ⎥ ⎡u ⎤ ⎢ 0 u z ⎥ C P = ⎢ y ⎥ ⇒ = λ ⎢ c ⎥ ⇒ = ⎢ c ⎥ ⎢ c ⎥ ⎢ ⎥ ⎢ ⎥ ⎣ y ⎦ ⎢ y c ⎥ ⎣ v ⎦ ⎢ y c ⎥ ⎢ ⎥ v 0 + λ k v ⎣ z c ⎦ ⎢ ⎥ ⎢ ⎥ ⎣ z c ⎦ pixels ⎣ z c ⎦ 8 European Summer University - Montpellier 2007 I.1I.1 BackgroundBackground andand definitionsdefinitions D. Camera projection model ⎡u⎤ ⎡λku 0 u0 ⎤⎡xc ⎤ ⎡xc ⎤ z ⎢v⎥ = ⎢ 0 λk v ⎥⎢y ⎥ = K ⎢y ⎥ c ⎢ ⎥ ⎢ v 0 ⎥⎢ c ⎥ ⎢ c ⎥ ⎣⎢1⎦⎥ ⎣⎢ 0 0 1 ⎦⎥⎣⎢zc ⎦⎥ ⎣⎢zc ⎦⎥ • Intrinsic camera parameters obtained by calibration • Mathematical model of a perfect optical system / physical phenomenon (distorsions, etc.) • Numerical sensor : aliasing, dynamical behaviour (limited bandwidth) • Other models apply for other types of « visual » sensors, e.g., C-arm, CT- scan, ultrasound probe, … 9 European Summer University - Montpellier 2007 OverviewOverview Part I : Fundamentals of visual servoing Background and definitions Servoing architectures and classification Position-based visual servoing Image-based visual servoing Visual servoing without feature extraction Part II : Medical robotics applications Laparoscopic surgery Internal organ motion tracking 10 European Summer University - Montpellier 2007 I.2 Classification I.21 Camera position A. Eye-in-hand configuration c Ht b He e Hc e Hc must be known (calibration) or the reference position must be learned (showing) 11 European Summer University - Montpellier 2007 I.2 Classification I.21 Camera position B. External camera configuration c He c He must be measured c b Ht or He b Hc must be known b Hc 12 European Summer University - Montpellier 2007 I.2 Classification I.22 Control level A. Indirect visual servoing Cartesian controller * * ∆p or r q* Trajectory Joint Power generator control amplifiers joint Control angles q Robot+camera law T Image - processing image •Suitable for slow visual servoing (T-1 < 50 Hz) •Control law is easier to design 13 European Summer University - Montpellier 2007 I.2 Classification I.22 Control level B. Direct visual servoing * τ * or q Control Low level - joint controllers law q Robot+camera T Image processing image •Suitable for fast visual servoing (T-1 >= 50 Hz) •Control law design is more complex (robot dynamics must be taken into account) 14 European Summer University - Montpellier 2007 I.2 Classification I.23 Feedback variables A. Position-based visual servoing (3D visual servoing) pd Power Control - amplifiers p Robot+camera T f Pose Features estimation extraction image •A model of the object must be known or multiple images should be used •Calibration errors may induce large pose estimation errors •Control law design is easier •Possible loss of target for large errors 15 European Summer University - Montpellier 2007 I.2 Classification I.23 Feedback variables B. Image-based visual servoing (2D visual servoing) f d Power Control - amplifiers Robot+camera f T Features image extraction •Smaller computational burden •Eliminates errors due to calibration •More complex control law •Workspace limits can be hit for large errors 16 European Summer University - Montpellier 2007 I.2 Classification I.23 Feedback variables C. Hybrid visual servoing (e.g. 2D1/2 visual servoing) fpd Power - Hybrid amplifiers ppd control law - Robot+camera fp f T Partial pose Features p p estimation extraction image •Smaller errors due to calibration •Simplified model of the target •Better properties of motion (in the image and in 3D) 17 European Summer University - Montpellier 2007 I.2 Classification I.24 Bandwidth of the visual servo-loop A. Slow visual servoing Sampling frequency < 50Hz Indirect visual servoing Robot transfer function model without dynamics Proportional control law (P) B. Fast visual servoing Sampling frequency >= 50 Hz Direct visual servoing Dynamical model of the robot must be taken into account More advanced control laws : PID, predictive, robust, non-linear, … 18 European Summer University - Montpellier 2007 OverviewOverview Part I : Fundamentals of visual servoing Background and definitions Servoing architectures and classification Position-based visual servoing Image-based visual servoing Visual servoing without feature extraction Part II : Medical robotics applications Laparoscopic surgery Internal organ motion tracking 19 European Summer University - Montpellier 2007 I.3 Position-based visual servoing I.31 Indirect visual servoing A. Control law p ep * or * d Control ∆p r Cartesian - law controller joint angles q pˆ = p + δp T fˆ Robot+camera Pose Features estimation extraction image •e• p generally expressed as translation vector and θu * • Look-then-move strategy (T very large, asynchronous) : ∆p = ep −1 • Pseudo-continuous strategy : p = J T r * T p Control law : r = k J p ep 20 European Summer University - Montpellier 2007 I.4 Image-based visual servoing I.41 Indirect visual servoing B. Obtaining reference pd • Reference position of the end-effector is given w.r.t an object Computation of the corresponding position of the camera w.r.t the object (eye-in-hand) e requires the knowledge of H c • Learning of the image in the final position and pose estimation of the camera 21 European Summer University - Montpellier 2007 I.3 Position-based visual servoing I.31 Indirect visual servoing C. Interaction matrix Jp Case of a eye-in-hand system c* ⎡ Tc ⎤ ep = ⎢ c* ⎥ ⎣ θu⎦ ⎡ c*C ⎤ ⎡ c*R c R (V + Ω × EC)⎤ ⎡ c*R c R −c*R c R EC⎤⎡V ⎤ c e e e c e c e e ep = ⎢ c* c* ⎥ = ⎢ c* c ⎥ = ⎢ c* c ⎥⎢ ⎥ ⎣ ( Ωc )⎦ ⎣ Rc ReΩe ⎦ ⎣ 0 Rc Re ⎦⎣Ωe ⎦ 22 European Summer University - Montpellier 2007 I.3 Position-based visual servoing I.31 Indirect visual servoing D. Pose estimation Camera calibration : Tsaï (IEEE Trans. Rob. Aut., 1987), Zhang ( ICCV 99) Pose estimation : Analytical : 3 or 4 points : Tsaï (co-planar target) Numerical Iterative : - DeMenthon (IEEE Trans. PAMI 1992, Int. J. Comp. Vision 1995), - VVS (Marchand, Eurographics 2002), - Minimization of reprojection error (gradient, Levenberg-Marquardt, etc.) 23 European Summer University - Montpellier 2007 I.3 Position-based visual servoing I.31 Indirect visual servoing E. Stability and robustness Stability is not an issue : Look-then-move strategy is always stable and low speed * T T −1 Exponential vision loop: r ≈ r p = k J p J p ep convergence Jp depends on camera position w.r. to end-effector (eye-in-hand configuration) Stability if ˆ −1 J P J P > 0 Measurement error is an issue: δp can be very large ! Main sources of uncertainty : ⇒Improvement: use learning Camera intrinsic parameters of pc by showing when possible Camera position w.r. to end-effector if eye-in-hand configuration Camera position w.r. to the robot base and robot kinematic chain if external camera, except if end-effector pose is estimated by vision Feature detection error : δf = fˆ − f 24 European Summer University - Montpellier 2007 I.3 Position-based visual servoing I.32 Direct visual servoing A.