Open Collections

UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

On the development of a heart motion compensation system on the da Vinci research kit for minimally invasive… Ruszkowski, Angelica 2015

Your browser doesn't seem to have a PDF viewer, please download the PDF to view this item.

Item Metadata


24-ubc_2015_november_ruszkowski_angelica.pdf [ 26.4MB ]
JSON: 24-1.0166664.json
JSON-LD: 24-1.0166664-ld.json
RDF/XML (Pretty): 24-1.0166664-rdf.xml
RDF/JSON: 24-1.0166664-rdf.json
Turtle: 24-1.0166664-turtle.txt
N-Triples: 24-1.0166664-rdf-ntriples.txt
Original Record: 24-1.0166664-source.json
Full Text

Full Text

On the Development of a Heart Motion Compensation Systemon the da Vinci Research Kit for Minimally Invasive Surgery onthe Beating HeartbyAngelica RuszkowskiBASc, University of Waterloo, 2013A THESIS SUBMITTED IN PARTIAL FULFILLMENTOF THE REQUIREMENTS FOR THE DEGREE OFMaster of Applied ScienceinTHE FACULTY OF GRADUATE AND POSTDOCTORAL STUDIES(Electrical and Computer Engineering)The University of British Columbia(Vancouver)August 2015c© Angelica Ruszkowski, 2015AbstractThis thesis describes the development of a heart motion compensation system on theda Vinci Research Kit for coronary artery bypass surgery. With this teleoperation roboticplatform, minimally invasive surgery on a beating heart could be performed on an alreadyclinically prevalent system. Semi-automation of the slave manipulators of the robot is in-troduced as they track the surface of the beating heart. The surgeons regular teleoperationcommands are superimposed on the automated trajectory. To achieve a virtually stabilizedenvironment, a novel concept of maintaining the camera fixed relative to the heart target isproposed.The preliminary research question is whether the robot is capable of tracking the highlydynamic heart motion. System identification was performed on the seven degree of freedomda Vinci slave manipulators, and an open loop controller was developed. The controlleris based on spectral line decomposition and the assumption of a periodic trajectory. Itsuccessfully commanded the slave manipulators to track an actual three dimensional hearttrajectory with submillimetre error. Experiments were conducted with expert robotic usersto evaluate surgeons’ ability to perform tasks on a moving target emulating the beatingheart, with very promising outcomes. The number of missed targets decreased from 37%to 13% when compensation was enabled, the number of hit targets increased from 26% to41%, and completion time decreased.A second generation system was developed which includes real time motion measure-ment commanding the robot. Results from user studies with expert surgeons performingbimanual suturing on moving targets with the new system support the motion compensa-tion. They also show the significance of motion measurement errors.iiAs an added safety, a virtual fixture was implemented to protect the heart from accidentalcollisions with the instrument tips. User studies were conducted to validate the efficacy ofthe fixture.To expedite controller development, an interface was developed between Matlab Simulinkand the C++ code that runs the da Vinci Research Kit. This allows on-the-fly testing of con-trollers which could be designed and developed in the convenient Simulink environment.Future work will be include closed loop control, improved experiment design, and theincorporation of electrocardiogram signals.iiiPrefaceI was the lead investigator for the work described herein. My supervisor, Dr. Salcudean,presented me with the project of heart motion compensation on the da Vinci Research Kit.The design, development, implementation and user studies on the proof of concept heartmotion compensation system were all conducted in the Robotics and Controls Laboratory(RCL) in the Electrical and Computer Engineering Department at the University of BritishColumbia, Point Grey Campus. I was granted a visiting student position at the CollaborativeHaptics and Robotics in Medicine (CHARM) Laboratory in the Mechanical EngineeringDepartment at Stanford University for the last quarter of 2014. At Stanford, I was theprimary researcher on two collaborative projects, one infrastructure-based and one research-based. Publications resulted from both of these, as described below. After Stanford, Ireturned to RCL where I continued to be the lead investigator in the completion of systemimprovements including the development of more advanced control techniques and moreclinically relevant user studies.A version of Section 5.3 was presented as a poster at the workshop discussing SharedFrameworks for Medical Robotics Research at the International Conference for Roboticsand Automation (ICRA) 2015 in Seattle, Washington [1]. As associated abstract was sub-mitted and is to be included in the workshop proceedings [Ruszkowski A., Quek, Z. F.,Okamura, A. M., and Salcudean, S. Simulink R©to C++ Interface for Controller Develop-ment on the da Vinci R©Research Kit (dVRK). ICRA 2015 Workshop: Shared Frameworksfor Medical Robotics Research]).A publication in the Proceedings for ICRA 2015 [Ruszkowski, A., Mohareri, O., Licht-enstein, S., Cook, R., and Salcudean, S. On the feasibility of heart motion compensation onthe daVinci R©surgical robot for coronary artery bypass surgery: Implementation and userivstudies. In Robotics and Automation (ICRA), 2015 IEEE International Conference on (pp.4432-4439). IEEE] has been partitioned into various sections of this thesis. The manuscriptwas edited for its inclusion herein to maintain the logical flow of the thesis. Controllerdevelopment details from the publication are included in Section 5.1, Section 5.4, and Sec-tion 5.5. The user studies, results and conclusions described in the publication are found inChapter 6. The system described in this publication was also demonstrated as an entry atthe first Surgical Robot Challenge at the 2015 Hamlyn Symposium for Medical Robotics inLondon, UK.A version of Chapter 7 has been presented as a poster with an associated abstract at the2015 Hamlyn Symposium for Medical Robotics [Ruszkowski A., Quek, Z. F., Okamura,A. M., and Salcudean, S. Dynamic Non-Continuous Virtual Fixtures for Operations on aBeating Heart Using the da Vinci R©Research Kit. 2015 Hamlyn Symposium on MedicalRobotics (pp. 45-46)]. Quek Z.F. contributed to the early stages of concept formation.For all of the above, as lead investigator for this work I was responsible for concept de-velopment, software implementation, user study design and execution, data collection andanalysis, and manuscript composition. Dr. Okamura co-supervised the research we con-ducted at Stanford, offering her expertise in haptics for concept formation. Dr. Salcudeanwas the supervisory author on this entire project and was integral throughout the projectin concept formation and development, manuscript edits, and most importantly technicalguidance.vTable of ContentsAbstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iiPreface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ivTable of Contents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viList of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xList of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiGlossary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xvAcknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xvii1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.1 Coronary Artery Bypass Grafting Surgery . . . . . . . . . . . . . . . . . . 11.2 The da Vinci Research Kit . . . . . . . . . . . . . . . . . . . . . . . . . . 21.3 Available Solutions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.4 Project Objectives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.5 Thesis Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.1 Coronary Artery Bypass Grafting . . . . . . . . . . . . . . . . . . . . . . . 62.1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.1.2 Procedure Description . . . . . . . . . . . . . . . . . . . . . . . . 72.1.3 Advances in CABG . . . . . . . . . . . . . . . . . . . . . . . . . . 82.2 Previous Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.2.1 Motion Measurement . . . . . . . . . . . . . . . . . . . . . . . . . 10vi2.2.2 Motion Tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . Passive Compensation Systems . . . . . . . . . . . . . . Active Compensation Systems . . . . . . . . . . . . . . Control Systems . . . . . . . . . . . . . . . . . . . . . . 152.3 Virtual Fixtures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173 Heart Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.1 Heart Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 203.2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.3 Spectral Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 244 Platform Used / The System . . . . . . . . . . . . . . . . . . . . . . . . . . . . 264.1 The da Vinci Research Kit . . . . . . . . . . . . . . . . . . . . . . . . . . 264.1.1 Mechanical Components . . . . . . . . . . . . . . . . . . . . . . . 274.1.2 Electrical Hardware . . . . . . . . . . . . . . . . . . . . . . . . . 294.1.3 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.2 Control Architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304.3 Robot Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 365.1 System Identification . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 365.2 PID Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405.3 Simulink to C++ Interface . . . . . . . . . . . . . . . . . . . . . . . . . . 425.3.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 425.3.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 435.3.2.1 Preserving the Component-Based Architecture . . . . . . 435.3.2.2 Threading . . . . . . . . . . . . . . . . . . . . . . . . . 455.3.2.3 Communicating with Matlab . . . . . . . . . . . . . . . 455.3.2.4 Switching Control . . . . . . . . . . . . . . . . . . . . . 465.3.3 Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 475.3.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 505.4 Spectral Line Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . 505.5 Visual Stabilization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 556 Initial User Studies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 576.1 User Studies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57vii6.1.1 Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 586.1.2 Participants and experiment completion . . . . . . . . . . . . . . . 596.1.3 Task Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 596.1.4 Results and Discussion . . . . . . . . . . . . . . . . . . . . . . . . 606.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 656.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 667 Virtual Fixtures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 677.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 687.2 Materials and Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . 687.2.1 Virtual Stabilization System Overview . . . . . . . . . . . . . . . . 687.2.2 Virtual Fixture Design . . . . . . . . . . . . . . . . . . . . . . . . 707.3 User Studies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 707.4 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 727.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 728 Incorporating Motion Measurement . . . . . . . . . . . . . . . . . . . . . . . 748.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 748.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 758.2.1 System Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . 758.2.2 The Heart: A Novint Falcon . . . . . . . . . . . . . . . . . . . . . 778.2.3 The Sensor: NDI Certus Optotrak . . . . . . . . . . . . . . . . . . 798.2.4 The Robot: the dVRK . . . . . . . . . . . . . . . . . . . . . . . . 808.2.5 Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 818.2.6 Dealing with Delay . . . . . . . . . . . . . . . . . . . . . . . . . . 848.2.7 Spectral Line Controller . . . . . . . . . . . . . . . . . . . . . . . 868.2.8 System Performance . . . . . . . . . . . . . . . . . . . . . . . . . 888.2.9 External Validation . . . . . . . . . . . . . . . . . . . . . . . . . . 938.3 User Studies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 948.3.1 Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 948.3.2 Task Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 948.3.2.1 Task Target . . . . . . . . . . . . . . . . . . . . . . . . . 948.3.2.2 Test Conditions . . . . . . . . . . . . . . . . . . . . . . 958.3.2.3 Metrics . . . . . . . . . . . . . . . . . . . . . . . . . . . 968.4 Results and Discussions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 988.4.1 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98viii8.4.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1038.5 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . . . . . 1049 Conclusion and Recommendations . . . . . . . . . . . . . . . . . . . . . . . . 1079.1 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1079.2 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1099.3 Recommendations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113A Running an Experiment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120B Software Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124B.1 SVN . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124B.1.1 HeartTrajectoryCode . . . . . . . . . . . . . . . . . . . . . . . . . 125B.1.2 LogDataAnalysis . . . . . . . . . . . . . . . . . . . . . . . . . . . 125B.1.3 NDICalibResults . . . . . . . . . . . . . . . . . . . . . . . . . . . 125B.1.4 NDICalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125B.1.5 NDIOAPI svn . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125B.2 GIT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125ixList of TablesTable 4.1 Control loop frequencies . . . . . . . . . . . . . . . . . . . . . . . . . . 32Table 5.1 Experimentally Determined Transfer Function Parameters for PSM Joints 40Table 5.2 Timing analysis for Simulink joint-level PID (PIDθ ) and Cartesian (PIDx)controllers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49Table 5.3 Ranked harmonics of the heart motion . . . . . . . . . . . . . . . . . . 51Table 5.4 Tracking Errors using Spectral Line Controller . . . . . . . . . . . . . . 53Table 8.1 Maximum Tracking Errors for Closed Loop Heart Motion Compensa-tion System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89xList of FiguresFigure 2.1 A graphic depicting the Coronary Artery Bypass Grafting (CABG) pro-cedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7Figure 2.2 A classic end-to-side anastomosis . . . . . . . . . . . . . . . . . . . . 8Figure 2.3 The highly invasive sternotomy . . . . . . . . . . . . . . . . . . . . . 9Figure 2.4 The fulcrum effect in the context of hand instruments of laparoscopicsurgery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10Figure 2.5 The Octopus by Medtronic . . . . . . . . . . . . . . . . . . . . . . . . 12Figure 2.6 The Cardiolock device . . . . . . . . . . . . . . . . . . . . . . . . . . 14Figure 2.7 The GyroLock device . . . . . . . . . . . . . . . . . . . . . . . . . . . 14Figure 3.1 Direction of axes of heart motion . . . . . . . . . . . . . . . . . . . . . 21Figure 3.2 Translation components of original versus rotated heart motion trajec-tories. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22Figure 3.3 Comparison of translational components of used heart trajectory. . . . . 22Figure 3.4 3D view of used heart trajectory. . . . . . . . . . . . . . . . . . . . . . 23Figure 3.5 Kinematics of heart trajectory. . . . . . . . . . . . . . . . . . . . . . . 25Figure 3.6 Spectral analysis of heart trajectory. . . . . . . . . . . . . . . . . . . . 25Figure 4.1 The dVRK System at UBC . . . . . . . . . . . . . . . . . . . . . . . . 27Figure 4.2 The Master Console of the da Vinci Classic . . . . . . . . . . . . . . . 28Figure 4.3 The Patient Side Manipulators of the da Vinci Classic . . . . . . . . . . 28Figure 4.4 Controller boxes for the dVRK . . . . . . . . . . . . . . . . . . . . . . 29Figure 4.5 Degrees of motion of a PSM . . . . . . . . . . . . . . . . . . . . . . . 32Figure 4.6 Remote center of motion on a PSM . . . . . . . . . . . . . . . . . . . 33Figure 4.7 DH Frames for the PSM and attached (Large Needle Driver) instrument 34Figure 5.1 Block diagram representation of the PSM motor plant . . . . . . . . . . 37xiFigure 5.2 Block diagram representation of the PSM motor plant controlled by PD 38Figure 5.3 Block diagram of a PID controller . . . . . . . . . . . . . . . . . . . . 41Figure 5.4 Control architecture for Simulink/C++ Interface . . . . . . . . . . . . . 44Figure 5.5 Custom Simulink block for receiving data in Simulink/C++ interface.The length of the output vectors can be customized. . . . . . . . . . . . 46Figure 5.6 Custom Simulink block for sending data in Simulink/C++ interface.torque is a <NumberOfJoints, 1> vector. . . . . . . . . . . . . . . . . 46Figure 5.7 Simunlink model of joint level PID control . . . . . . . . . . . . . . . 48Figure 5.8 Evaluation of heart motion reconstruction (total error over one period)versus number of spectral lines . . . . . . . . . . . . . . . . . . . . . . 52Figure 5.9 Difference between original and reconstructed heart trajectories usingN = 4 spectral lines . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52Figure 5.10 Commanded and measured trajectories over five periods . . . . . . . . 54Figure 5.11 Tracking error for heart motion compensation over five periods . . . . . 54Figure 5.12 Camera and 3D printed housing . . . . . . . . . . . . . . . . . . . . . 55Figure 5.13 Camera mounts for visual stabilization . . . . . . . . . . . . . . . . . . 56Figure 6.1 Task 1 : Simulated suture . . . . . . . . . . . . . . . . . . . . . . . . . 60Figure 6.2 Task 2 : Peg and ring task . . . . . . . . . . . . . . . . . . . . . . . . 61Figure 6.3 Results of Suture Task. Top-left: Average time of completion. Top-right: Average accuracy/distance from center of target. Bottom-left:Number of targets hit with an error of <0.5 mm. Bottom-right: Numberof targets missed with an error of ≥1.0 mm . . . . . . . . . . . . . . . 61Figure 6.4 Tradeoff between completion time and accuracy for suture task . . . . . 62Figure 6.5 Summary of inter-user results for accuracy and completion time forsuture task . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63Figure 6.6 Results of Peg Task. Left: Average time of completion. Right: Averagenumber of mistakes . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63Figure 7.1 dVRK Setup at Stanford University . . . . . . . . . . . . . . . . . . . 69Figure 7.2 High risk versus low risk periods of the heart trajectory . . . . . . . . . 69Figure 7.3 Tasks completed for virtual fixture user studies . . . . . . . . . . . . . 71Figure 7.4 Results for virtual fixture user studies . . . . . . . . . . . . . . . . . . 73Figure 8.1 Photograph 1 of generation two of the heart motion compensation System 75Figure 8.2 Photograph 2 of generation two of the heart motion compensation system 76xiiFigure 8.3 Block diagram of generation two of the Heart Motion Compensationsystem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76Figure 8.4 The Novint Falcon . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77Figure 8.5 Simulink control scheme for the Novint Falcon . . . . . . . . . . . . . 78Figure 8.6 NDI Optotrak Certus . . . . . . . . . . . . . . . . . . . . . . . . . . . 79Figure 8.7 NDI Optotrak Certus marker configurations . . . . . . . . . . . . . . . 80Figure 8.8 Windows of the GUI for the second generation heart motion compen-sation system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81Figure 8.9 Automated calibration trajectory for the dVRK . . . . . . . . . . . . . 82Figure 8.10 Block diagram of compensating for the system delay . . . . . . . . . . 85Figure 8.11 Received, commanded, and measured position signals for the dVRK . . 85Figure 8.12 Tracking error between heart target and dVRK position with and with-out delay compensation . . . . . . . . . . . . . . . . . . . . . . . . . . 86Figure 8.13 Plant for system identification in spectral line controller development . 86Figure 8.14 Bode plots for the plant depicted in Figure 8.13 . . . . . . . . . . . . . 87Figure 8.15 Comparing the input to the dVRK when spectral line control is/is notused on the Novint . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88Figure 8.16 Commanded versus achieved position signals for the Novint Falcon . . 90Figure 8.17 Tracking error between commanded and achieved position signals forthe Novint Falcon . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90Figure 8.18 Differences in position signals in the motion measurement component . 91Figure 8.19 Error between NDI measured position and Novint self-reported posi-tion, both transformed into the dVRK space (i.e. TNDItodV RK · yN(t)−yN,dV RK) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91Figure 8.20 Commanded versus achieved position signals for the dVRK PSM . . . 92Figure 8.21 Tracking error between commanded and achieved position signals forthe dVRK PSM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92Figure 8.22 External validation of dVRK PSM following a heart trajectory . . . . . 93Figure 8.23 User performing a suturing task . . . . . . . . . . . . . . . . . . . . . 95Figure 8.24 User study task phantom . . . . . . . . . . . . . . . . . . . . . . . . . 96Figure 8.25 Steps for tying the suture knot . . . . . . . . . . . . . . . . . . . . . . 97Figure 8.26 Completed task phantom . . . . . . . . . . . . . . . . . . . . . . . . . 97Figure 8.27 Suturing results: Time of completion box plot . . . . . . . . . . . . . . 99Figure 8.28 Suturing results: Time of completion scatter plot. . . . . . . . . . . . . 100Figure 8.29 Suturing results: MTM displacement . . . . . . . . . . . . . . . . . . 100xiiiFigure 8.30 Suturing results: Accuracy . . . . . . . . . . . . . . . . . . . . . . . . 101Figure 8.31 Suturing results: Number of errors . . . . . . . . . . . . . . . . . . . . 102Figure 8.32 Suturing results: Likert scale . . . . . . . . . . . . . . . . . . . . . . . 102xivGlossaryAOB Active ObserverAPI Application Programmer’s InterfaceARMC Active Relative Motion Cancellingbpm Beats per MinuteCABG Coronary Artery Bypass GraftingCPB Cardiopulmonary BypassDH Denavit-HartenbergDOF Degree of FreedomdVRK da Vinci Research KitECABG Endoscopic Coronary Artery Bypass GraftingECG ElectrocardiogramECM Endoscopic Camera ManipulatorEKF Extended Kalman FilterFDA Food and Drug AssociationFIFO First In First OutFPGA Field Programmable Gate Arrayfps Frames Per SecondxvHIP Haptic Interface PointICRA International Conference for Robotics and AutomationMCI Motion Compensation InstrumentMPC Model Predictive ControlMRI Magnetic Resonance ImageMTM Master Tool ManipulatorOPCABG Off-Pump Coronary Artery Bypass GraftingPCA Principal Component AnalysisPCI Percutaneous Coronary InterventionPD Proportional-DerivativePID Proportional-Integral-DerivativePSM Patient Side ManipulatorQLA Quad Linear AmplifierRCM Remote Center of MotionRNEA Recursive Newton Euler AlgorithmRHMPC Receding Horizon Model Predictive ControlSISO Single Input Single OutputSVD Singular Value DecompositionTPS Thin-Plate SplineUS UltrasoundxviAcknowledgmentsFirst, I would like to express my deepest gratitude to my supervisor Dr. Tim Salcudean,for giving me the opportunity of working on one of the most fascinating projects on theabsolute coolest toy in perhaps the most beautiful city in the world. Dr. Salcudean’s brillianttechnical guidance and support was integral to the successful completion of this project.Thank you for all the advice, assistance with formulating battle plans, letting me run freeand then helping me when I got completely stuck. Also, thank you to both Dr. Salcudeanand my temporary co-supervisor, Dr. Okamura, for the invaluable experience of being aVisiting Student at Stanford University.I would also like to share my sincere appreciation for our clinical collaborators, namelyDr. Sam Lichtenstein, Dr. Richard Cook, Dr. Larry Goldenberg, Dr. Peter Black, and Dr.Chris Nguan. It is incredible to have such support on the clinical side, and enthralling tohave had expert robotic surgeons testing (and liking!) my system.The financial support from NSERC CGS-M, NSERC MSFSS and Microsoft ResearchWomen’s Scholarship Program, as well as from CFI, the C.A. Laszlo Chair, and NSF Grant#1227406, is all very gratefully acknowledged.I would like to recognize and express my appreciation to Omid Mohareri and Zhan Fan.These two PhDs were the da Vinci robot masters at UBC and Stanford, respectively, andI owe them many many thanks. On the topic of the robot, I must also recognize PeterKazanzides and Anton Deguet from Johns Hopkins University, for all your amazing workand support on the dVRK. It was especially appreciated all the times I managed to break therobot. The incredible contribution of Simon diMaio must also be acknowledged; from lend-ing me two prototype camera systems to helping me with robot maintenance and technicalxviiquestions about the robot, thank you for everything.I have to thank all my Vancouver/VOC friends and RCL labmates, particularly the foos-ball squad, the Spartacus crew, and my late night/weekend lab buddies. You’ve made myexperience at UBC an amazing one. A very special thanks to Caitlin Schneider and JulioLobo. These two helped me maintain my sanity and get through my first user studies withthe surgeons. Caitlin, also thanks for pulling me into the VOC before I even got here andfor all the outdoors advice and suggestions. I’m going to miss this playground aroundVancouver immensely.Also, here’s to the UBC Figure Skating Club whose pilot program ran my last springterm here! Best of luck in the first official years of the club!My deepest love and gratefulness is felt for my parents Barbara and Kazimierz Ruszkowski,my sister Dominika and brother Sebastian. Thank you for all the encouragement, love,Skype calls and support when I was so far from home. To Mama and Tata, I cannot reallyexpress my gratitude for everything you have been through, everything you’ve sacrificed,everything you’ve done to get me to where I am. You are my heroes and role models.Bardzo bardzo was kocham, dwa razy bardziej.Finally, to my dearest Brian, thank you for your extraordinary love and support. Despitethe long distance, you helped me get through these past two years. From helping me un-derstand controls concepts, to patiently listening to my rants about the lab; from being aneverending source of comfort when things got tough to listening to all my excited talesof my adventures here – you are always there for me. Thank you, my Love, it’s been anintense two years, I would have lost my sanity long ago if not for you.xviiiChapter 1Introduction1.1 Coronary Artery Bypass Grafting SurgeryCardiovascular diseases are the leading cause of death in the world, accounting for nearlyone third of all global deaths in 2008 [2]. There is also a significant economic impact sus-tained from cardiovascular disease. In 2009, a total of $312.6 billion direct and indirectcosts were incurred in the United States alone [3]. In advanced coronary artery diseases,where a blockage develops in one of the coronary arteries, the most common surgical revas-cularization procedure is called Coronary Artery Bypass Grafting (CABG) Surgery.As the name indicates, the objective of the CABG procedure is to bypass a blocked coro-nary artery. This is accomplished by harvesting an artery from elsewhere in the patient andthen attaching (or grafting) it to the surface of the heart to bypass the blockage. The vesselsinvolved are typically in the order of 2 mm or less in diameter, and so the procedure requiresvery fine precision. To make this task feasible for the surgeon, the heart is normally stoppedat the patient is put on a Cardiopulmonary Bypass (CPB) machine. It has been documented,however, that the use of the CPB incurs significant possibility of complications includingcognitive loss and increased hospitalization time and costs [4]. Moreover, a sternotomy isperformed to access the heart. This involves a large incision and cracking the sternum – ahighly painful procedure with long recovery times.The achievement of minimally invasive beating heart surgery would be greatly advanta-geous for the patient, eliminating the need for both the sternotomy and the use of the CPB1machine. The concept of heartbeat synchronization, or motion cancellation, is promisingfor achieving this goal. By devising a system which can measure the heart motion and trackit, the instruments could be commanded to keep a fixed distance from the beating heartsurface. The surgeon’s motions would thus be relative to the tissue, and ideally the highprecision surgical manoeuvres could be preserved.1.2 The da Vinci Research KitThe motion tracking component of a heart motion compensation system lends itself per-fectly to a robotic solution. Programming a robot to automatically compensate for the heartmotion would provide a feasible solution to the challenge of minimally invasive beatingheart surgery, assuming the robot was capable of following the highly dynamic motion ofthe heart.The da Vinci R©Surgical System from Intuitive Surgical Inc. in Sunnyvale, California isalready a clinically prevalent surgical robot. Thousands are presently in use in hospitals,with over half a million robotic procedures having being performed in 2014 alone. Theda Vinci is a teleoperation robot; it comprises of two major components, the master andslave. The surgeon sits at the Master Console, typically a few feet away from the patient,and operates the Master Tool Manipulator (MTM)s. The surgeon’s motions are read by therobot and translated to the patient by way of the Patient Side Manipulator (PSM)s. Therobot performs filtering to remove the surgeon’s hand tremors, motion scaling to permithigh precision motions on the slave side, and kinematic transformations that align the sur-geon’s physical hand motions with the motions of the instruments in the camera feed. Theinstruments are articulated and offer high degrees of dexterity in the minimally invasive en-vironment. The vision system provides the da Vinci operator with a 3D view of the surgicalfield.In collaboration with Intuitive Surgical Inc., Johns Hopkins University and WorcesterPolytechnic Institute have developed the da Vinci Research Kit (dVRK). The dVRK is amechatronics telerobotic surgical research platform comprising of mechanical componentsfrom the first generation da Vinci Surgical System (the da Vinci Classic), as well as elec-tronic hardware and software offering researchers access to all levels of control of the robot.Using this platform, researchers are able to experiment with new control and automation2schemes.1.3 Available SolutionsSince the benefits of minimally invasive beating heart surgery are great, much work hasgone into trying to accomplish this challenge. Clinically, Endoscopic Coronary Artery By-pass Grafting (ECABG) is attempted using mechanical stabilizers such as the Octopus R©tissuestabilizers. Instead of actively compensating for the heart motion, these devices suppress theheart motion by suctioning themselves down to the heart surface and restricting the move-ment of tissue. Studies have shown that the residual motion of these devices is still too large(about 1.52.4 mm), and safety and graft patency remain major concerns [5],[6],[7]. Activemechanical stabilizers have been developed to try to alleviate these concerns [8]. Epi-cardial crawling robots offer an interesting solution to compensating for the heart motion[9]. A number of custom heart motion compensation systems have also been developed[10],[11],[12],[13]. For the aforementioned solutions, however, clinically feasibility andthe ability to acquire approval from the Food and Drug Association (FDA) for clinical useremain major challenges.1.4 Project ObjectivesThe goal of this thesis is to to perform a feasibility study on achieving minimally invasivebeating heart surgery using the dVRK as a platform. This is a novel use of the da Vinci andthe project will include• a detailed evaluation of the robotic platform to validate its capability to track thecomplex, highly dynamic heart motion with sufficient accuracy.• controlling the PSMs of the da Vinci to automatically track the heart motion. Thiswill keep the instruments at a fixed distance from the beating heart surface.• the development of specialized controllers to achieve submillimetre tracking errors.• achieving visual stabilization with a small stereo endoscopic camera which will alsotrack the heart motion, to provide the robot operator the illusion of working in avirtually stabilized field.3• when working on a moving target, emulating performing surgery on a stopped heartby superimposing the surgeon’s teleoperation commands on the automated trajecto-ries.• incorporating motion measurement for real time control of the da Vinci PSM basedon the measured position of the heart surface• completing user studies to evaluate the participants’ abilities to use the proposed heartmotion compensation system to accomplish tasks on a moving target.The main advantage of this work is its clinical feasibility; it is being developed on the daVinci system which is already clinically prevalent. With an existing widespread presence inclinical practice, the da Vinci is poised to be a platform through which minimally invasivesurgery on the beating heart could have a significant impact on cardiovascular procedures.1.5 Thesis OverviewThe structure of this thesis is organized as follows:• Chapter 1 Introduction: Discusses the motivation and objectives of this thesis.• Chapter 2 Literature Review: Provides a background for the targeted clinical pro-cedure, discusses previously proposed solutions for motion compensation, and de-scribes the technical challenges of minimally invasive surgery on the beating heart.• Chapter 3 Heart Motion: Offers a description of the heart trajectory used in this study,including data processing and kinematics.• Chapter 4 Platform Used / The System: Provides a detailed description of the roboticplatform, including its configuration, kinematic and dynamics, and control architec-ture.• Chapter 5 Controller Design: Explains the design and development of the variouscontrollers used to achieve high accuracy heart motion tracking.• Chapter 6 Initial User Studies: Discusses the design and implementation of the initialproof of concept heart motion compensation system and corresponding user studies.• Chapter 7 Virtual Fixtures: Describes the development and evaluation of active con-straints that aim to improve safety of the heart motion compensation system.4• Chapter 8 Incorporating Motion Measurement: Details the extension of the system toincorporate real time motion measurement, moving towards clinical feasibility.• Chapter 9 Conclusions and Recommendations: Concludes the thesis with an overviewof this project. Also suggests recommendations for future improvements and devel-opments.5Chapter 2Literature ReviewThis chapter presents a review of the literature relevant to this project. The first sectiondescribes the target application, CABG, in more detail. Next, pertinent previous worktowards heart motion compensation systems are discussed. The last two sections explorethe control systems that have been previously developed for cardiac applications.2.1 Coronary Artery Bypass Grafting2.1.1 BackgroundCoronary heart disease is a disease in which blood flow to the heart is reduced due toa plaque build up inside the coronary arteries. The arteries are very narrow blood vesselsthat carry nutrients and oxygen to the heart. When the occlusion in the arteries builds, theflow of oxygen is reduced and blood clots may form, potentially completely blocking bloodflow. Angina and cardiac arrests may both be caused by such blockages [14].If the coronary artery disease is not too advanced, a nonsurgical procedure called Percu-taneous Coronary Intervention (PCI) (formerly known as angioplasty with a stent) may beperformed. PCI uses a catheter to insert a stent (a small mesh balloon that supports the in-ner artery wall) to open up blood vessels [15]. In some cases PCI is insufficient to improveblood flow through the arteries and the more invasive CABG procedure must be performed.6CABG is the most common open-heart surgery, with upwards of 400,000 patients in the2009 in the United States [16]. It is an highly effective treatment with typically excellent re-sults. Two large, independent studies (FREEDOM and SYNTAX) were recently performedand both concluded that CABG should be the preferred revascularization strategy for treat-ing coronary artery disease [17],[18]. This would suggest a rising number of future casesand greater potential impact for a significant improvement in the procedure.2.1.2 Procedure DescriptionThe CABG procedure aims to bypass a blockage formed from plaque build up inside thecoronary artery. This is accomplished by harvesting an artery from elsewhere in the patientand attaching, or grafting, it to the surface of the heart to bypass the blockage. A diagramis seen in Figure 2.1. The saphenous vein from the leg and the left internal thoracic arteryare most common bypass grafts, with the latter being preferred since they tend to remainopen longer than venous grafts [21]. One end of the graft is attached to the aorta, and theother to the coronary artery distal to the blockage by way of an end-to-side anastomosis(Figure 2.2).The vessels being anastomosed in the CABG procedure are in the order of 2 mm indiameter. Thus great precision is required to complete the suture task. To accomplish this,Figure 2.1: A graphic depicting the CABG procedure. Venous or arterial grafts aremade to bypass blockages in the coronary artery [19]7Figure 2.2: A classic end-to-side anastomosis [20].the heart is usually stopped and the patient is put on a CPB machine. The CPB providesphysiological support with an extracorporeal circuit, resulting in a motionless, bloodlesssurgical field. The circuit drains blood from the heart and lungs via venous cannulation andtubing, oxygenates it, and returns the re-oxygenated blood to the cannulated arterial systemin a constant flow using a mechanical pump and artificial lung [22]. Use of the CPB resultsin a series of secondary damaging effects, including anemia, red blood cell aggregation,gaseous and particulate emboli, hemolysis (red blood cell damage), and localized ischemia[4].Since CABG is traditionally an open procedure, a sternotomy is performed in order toaccess the heart. This procedure involves a several inch long incision down the center ofthe chest and cracking the sternum open, seen in Figure 2.3. This is a highly painful andinvasive procedure, often requiring months of recovery.2.1.3 Advances in CABGThe invasiveness of traditional CABG surgery serves as motivation to investigate twomajor changes in the procedure. The first major change eliminates the need for the CPB8Figure 2.3: The highly invasive sternotomy [23].machine. Termed Off-Pump Coronary Artery Bypass Grafting (OPCABG), this means per-forming the surgery on the beating heart. The motion of the heart is a highly complex,location-dependent, six dimensional motion. Secondly, to eliminate the need for the in-vasive sternotomy, there have been efforts to proliferate minimally invasive, or ECABG.For such a procedure, a number of small incisions would be made between the ribs, andthe heart would be accessed using long shafted instruments in a lapascopic setting. Withclassic laparoscopy this would be exceedingly challenging due to the reduced mobility, vis-ibility and ergonomics, as well as the introduction of the fulcrum effect. In the context oflaparoscopic surgery, seen in Figure 2.4, the fulcrum effect results in magnified hand mo-tions (including hand tremors) and the direction of the tool tip is inverted from the directionof hand motion. Despite these challenges, the potential advantages of minimally invasivesurgery are compelling, including reduced patient trauma and lowered surgery costs.Robotic techniques have proven very successful for reducing the complexity for surgeonsin minimally invasive surgery, particularly in urological procedures. The da Vinci R©SurgicalSystem, specifically, is a teleoperation robot already in clinical use. The da Vinci has theability to filter out hand tremors, scale the surgeon’s motions, provide increased dexterityin the workspace through wristed, articulated instruments, and intelligent controls whichalign the surgeon’s physical motions to the instrument motion in the camera feed. A studyin Beijing attempted to perform the entire CABG procedure minimally invasively on thebeating heart with the da Vinci, but there was a substantial learning curve for even the ex-pert robotic surgeon [7]. As a compromise, a cardiothoracic surgeon at Vancouver General9Figure 2.4: The fulcrum effect in the context of hand instruments of laparoscopicsurgery [24].Hospital performs the majority of the CABG surgery robotically, however for the actualanastomosis task, a minor sternotomy must still be performed and the patient is still put onthe CPB machine. Should there be a system capable of performing the anastomosis on thebeating heart, off-pump ECABG could be achieved.2.2 Previous WorkA complete active heart motion compensation system would consist of two major com-ponents: a motion measurement component and a motion tracking component. The formerwould use a sensor technology that is able to read the position of the heart surface tissuein space and relay this information to some controller. The controller would use this datato command the device responsible for physically tracking the heart surface. Each of thesecomponents involves considerable complexity. The major focus of this thesis is on the mo-tion tracking component, and as such this was more thoroughly investigated in the literature.Some work on motion measurement will still be discussed, though briefly.2.2.1 Motion MeasurementSome of the earliest attempts at measuring the motion of the heart surface involve visionsystems (cameras) and embedded markers positioned at strategic locations (e.g. bifurca-tion points of the coronary arterial tree) on the heart surface [25]. [26] was one of the10first groups to perform feature-based soft tissue motion tracking of the heart surface. Softtissue tracking presents several challenges, namely the tissue is deformable, there are spec-ular reflectances from the bright camera lighting, occlusions from instruments or blood,as well as a general lack of distinctive features. The approach taken by Stoyanov et al.[26] to measure the movement of the tissue is to track 3D points in stereo images froma pre-calibrated stereo laparoscope using stereo-temporal constraints and an iterative reg-istration algorithm. Their salient landmark selection employs maximally stable extremalregions based on thresholding the image intensity, as well as traditional gradient-based im-age features to create connected components. Temporal motion tracking is achieved usinga Lucas-Kanade tracker and registration.An efficient algorithm for 3D tracking of the heart surface based on a Thin-Plate Spline(TPS) deformable model and an illumination compensation algorithm was proposed byRicha et al. [27]. A vision-based technique, the TPS warping defines a mapping betweeneach pixel to the current image of the surface, using a parametrization of projective depthsof the tracked surface to represent projective deformations. Illumination parametrization isalso incorporated. An efficient second-order minimization provides a fast and large conver-gence basin. Initial results were very promising, but there is a trade off between the numberof control points and computation costs. With a lower number of control points, occlusionsbecome more of an issue.Kurz et al. discuss a multiple target tracking technique in [28]. To achieve precise track-ing of the heart surface, 24 identical artificial markers are placed on the heart. To determinewhich measurement came from which marker, the multiple target tracking method is basedon a symmetric kernel equation transformation. The showed the algorithm capable of reli-ably tracking 24 unlabeled targets in real time. Clinical issues such as instrument or bloodocclusion of the markers, as well as placement of the markers, remain practical concerns.2.2.2 Motion TrackingThis section will provide an overview of existing/attempted compensation systems (bothpassive and active approaches will be discussed). These compensation systems are typicallymechatronics systems consisting of mechanical hardware, electronics and software for con-trol. There will also be a discussion of more advanced control techniques that have beeninvestigated.11Figure 2.5: The Octopus by Medtronic; example of a passive compensation device[29]. Passive Compensation SystemsMechanical surgical stabilizers are a type of device in clinical use which could be de-scribed as passive compensation systems. The Octopus R©, developed by Medtronic [29], isa common example, seen in Figure 2.5. A similar device was developed by the Universityof British Columbia [30]. These stabilizers use suction or vacuum pressure to clamp downon the heart tissue surrounding the surgical site, intending to stabilize the local tissue. Thereare risks of damaging the tissue if too much vaccum/suction force is used, and studies haveshown the residual motion (about 1.52.4 mm) is too great to achieve the accuracy requiredfor highly effective anastomoses [5],[6].The HeartLander robot, developed by Riviere et al. [9] is a very interesting conceptof an epicardial crawling robot which eliminates the need for active motion compensa-tion. A small robot with a magnetic tracking sensor traverses the surface of the heart us-ing inchworm-like locomotion. By modelling the physiological motion as a time-varyingFourier series using an Extended Kalman Filter (EKF) framework, more accurate positionestimation as well as synchronization of motion to the physiological cycles (both respiratoryand cardiac) can be achieved. Such a device is not directly applicable to CABG surgery, butis useful for modelling heart motion and administering of treatments locally. Active Compensation SystemsAn active compensation system in this context is a system which addresses mechani-cal stabilization, image stabilization, and shared control in order to perform beating heartsurgery. One of the earliest attempts at building such a system was by Trejos et al. [10].12The proposed system involved a hand/tool support which moved the surgeon’s hands totrack the motion of the heart. The device consisted of a specifically designed 3 Degree ofFreedom (DOF) mechanism with prismatic joints, a custom made mechanical sensor, and asuitable control strategy comprising of Proportional-Integral-Derivative (PID) control andcomputed torque control. A simulated suture task was attempted in a user study and it wasdemonstrated that moving the hand in synchrony with the task space achieved significantimprovement in accuracy over the uncompensated version. An increase in task completiontime was also observed.Nakamura et al. [12] introduced the concept of heartbeat synchronization for minimallyinvasive cardiac surgery by building a custom teleoperation robot. A Phantom Desktophaptic device (by SensAble Technologies Inc.) was adopted as the master device, and aprototype 4 DOF arm was used for the slave. The visual synchronization was done elec-tronically, processing the camera image by cutting and translating it so a laser-indicatedreference point always remained in the same position. Motion measurement was achievedusing a 955 Frames Per Second (fps) high speed camera to measure vibration of the laserpointer. Experimental results were very promising, unfortunately this cutting edge systemwas not developed further by this group.Instead of making a teleoperation robot, Yuen et al. [11] consider an actuated handheld1 DOF Motion Compensation Instrument (MCI). This work investigates the approximationof tissue motion as a 1D motion model, specifically the motion of the mitral valve annulus.3D Ultrasound (US) images are utilized to perform the motion measurement, however theseintroduce substantial temporal delays. A predictive filter, the EKF, exploits the quasiperiod-icity in the cardiac motion and is used to compensate for these delays. Using a feedforwardtrajectory of the cardiac target, user study results demonstrated that the 1D motion synchro-nization MCI allowed users to operate with increased dexterity and reduced forces on thesimulated heart surface.Another approach that builds upon the passive cardiac stabilizers discussed previously isone which introduces active compensation to eliminate the residual motion in the passivestabilizers. In [8] two such active stabilizers are presented: the Cardiolock and the Gy-roLock systems. Cardiolock introduces a piezo-actuator in the stabilizer structure, whileGyroLock works on the principle of the generation of gyroscopic torque. The cardiac stabi-lizers are inserted on a long stabilizer rod, and are tasked with stabilizing the heart surface13Figure 2.6: The Cardiolock device [8]in 2 DOF, those perpendicular to the beam. In the Cardiolock, shown in Figure 2.6, highdynamic actuators, piezoelectric actuators on a slide-crank mechanism, are introduced be-tween the stabilizer rod and its base. This mechanism controls the configuration of thecompliant mechanism to cancel any stabilizer tip displacement, which is observed usingthe endoscopic camera. A custom end-effector is added to the instrument, hosting a 6 DOFforce sensor and visual marker. The closed loop controller accomplished submillimetrestandard deviation of the residual displacement. A very enticing solution, clinical feasi-bility will be dependent on the quality and framerate of the endoscopic camera feed. TheGyroLock, in Figure 2.7, is designed to plug onto commercial passive stabilizers withoutmodification. Operating based on inertial effects, the GryoLock employs the principle ofControl Moment Gyroscope actuation, utilizing acceleration measurements taken from asensor embedded at the stabilizer tip. By controlling the inclination of a gyroscope spin-Figure 2.7: The GryoLock device [8]14ning at a constant high speed, an alternating torque can be generated which allows for thecompensation of periodic motion. An adaptive algorithm dedicated to harmonic disturbancerejection is used for the control law. Experimental results showed the reduction of displace-ment is sufficient for heart stabilization. Another highly interesting solution, however theauthors did not discuss the ease of modification of the commercially available stabilizer,nor a medical expert’s perspectives on whether the large device will be welcomed in thealready cramped operating theatre.Finally, a system named the MicroSurge is being developed by the DLR Institute ofRobotics and Mechatronics in Germany [31]. The system is designed to achieve a high de-gree of versatility in minimally invasive surgery, performing bimanual endoscopic telesurgerywith force feedback. Literature describing the design of the system – arms, instruments,master workstation, planning, etc. – is available, however experimental results for specificapplications have not yet been disclosed. With an ultimate ambition to perform robot-supported surgery on the beating heart, this is a group to watch for future innovations.While very creative and often promising, the majority of these systems cannot be clin-ically feasible without considering the process of obtaining FDA approval, which is bothhighly challenging and time consuming. Control SystemsA number of groups focused more intently on the intelligent control algorithms forrobotic-assisted beating heart surgery than on the design of the mechatronics system it-self. These algorithms incorporate prediction, different sensing technologies, and advancedshared control paradigms required for a heart motion compensation system.In [13], Bebek et al. introduce the Active Relative Motion Cancelling (ARMC) algorithmto cancel the relative motion between the surgical instruments and a point of interest on thebeating heart, dynamically stabilizing the heart. This is a model-based algorithm whichemploys the novel use of biological signals, specifically the Electrocardiogram (ECG), toachieve motion cancellation. The motion tracking problem is rephrased as a reference sig-nal estimation problem with the help of a model predictive controller. Taking advantageof the quasiperiodicity of the heart motion, the estimated signal is derived from the previ-ous cardiac cycle. To compensate for errors introduced by heart rate variations, the ECG15wave forms are incorporated into the controller to adjust the tracking. It was noted by theauthors that using the ECG signal would be insufficient for severe rhythm abnormalities,occurring due to arrhythmias for example. Bebek et al. also discuss the incorporation ofother biological signals, including aortic, atrial and ventricular blood pressure.These concepts are extended in [32], moving more towards prediction of the heart motionfor their adaptive estimation algorithms. The proposed algorithm uses a specialized adap-tive filter to generate future position estimates. A sonomicrometry system was used as theprimary sensing technology. Based on a vector autoregressive model, recursive least squaresis used to update the filter weights, with past observations being exponentially windowed.The predictive controller effectively implements Receding Horizon Model Predictive Con-trol (RHMPC) in the feedforward path, and outperforms an EKF-based algorithm in termsof tracking performance. The EKF performed better in the presence of high-measurementnoise and variations in the heart rate, however. The tradeoff in this method is between thenoise in the estimator and the computational load.A fast adaptive approach which can better handle extreme irregularities in the heart mo-tion is presented by Liang et al. [33]. This adaptive nonlinear heart motion model is basedon the Volterra Series. The Volterra Series is widely used in physiology signal estimationand modelling, naturally derived from the nonlinear dynamics of heart motion, offeringgood estimation of the phase-coupling phenomenon in heart motion dynamics. Liang etal. investigate these phase coupling effects between respiration and cardiac motion usingFourier and bi-spectral analyses. It was found that the nonlinear quadratic coupling regu-lates the dynamics of the beating heart and can improve the prediction accuracy by coveringsharp change points resulting from even extreme irregularities in the heart motion.Moving from predictive to force control, in Kesner et al. [34], 3D ultrasound guidanceand position-modulated force control are used to command a robotic catheter system to ap-ply a constant force against a moving target. The 3D US is capable of imaging and trackingboth the catheter and the soft tissue, and these measurements are fed into an EKF that esti-mates the current tissue location based on a Fourier decomposition of the cardiac cycle. Aclever force sensor is designed with a fiber optic transducer, and provides feedback to thecontroller which adjusts the catheter motion due to the forces experienced by the cathetertip while interacting with the moving target. Both Coulombic friction and backlash com-pensation models are employed in the controller, however the parameters of these models16are very system dependent.The recent work described in [35] combines force feedback (does not require visiondata), adaptive control and predictive control in a novel robotic control architecture forbeating heart motion compensation. The controller consists of two cascade loops: the innerloop is model-reference adaptive control loop based on the Kalman Active Observer (AOB),and the outer loop is based on a Model Predictive Control (MPC) approach that generatescontrol references. The AOB inner-loop imposes the desired and stable closed-loop dynam-ics, based on non-linear feedback linearization, augmented state-feedback, and stochasticdesign. The MPC outer-loop generates force references for AOB control by predicting theapplied force in a finite time horizon. The robotic arm used in the validation study, a 4DOF WAM arm from Barrett Technology, is equipped with a 6 DOF JR3 force sensor. Thisprocedure neglects torsional ventricular motion.While the work with force control presents promising results, the practical considerationsof developing a surgically feasible force sensor that can be easily incorporated in in vivoapplications, survive sterilization procedures and still function in the messy clinical settingcast doubt on the clinical feasibility of these solutions. For the other control schemes, thechallenge is to transfer these clever strategies to a plant/robot which will be stable with sucha controller, and also allow the surgeon to perform complex tasks such as anastomoses.2.3 Virtual FixturesAnother interesting controls concept surrounding beating heart surgery is that of virtualfixtures, or active constraints. Virtual fixtures enable robots to intelligently collaborate withhuman users to complete tasks that are too complex for the robot to complete autonomouslybut too challenging for the user to complete unassisted. This is not a new concept, howeverthe body of work on dynamic virtual fixtures, specifically in the context of teleoperationand beating heart surgery is not extensive.The first reported attempt to employ dynamic virtual fixtures for cardiac surgery wasachieved by Park et al. [36]. In this work, the virtual fixtures were meant to assist in theartery harvesting portion of a robot-assisted CABG procedures. The virtual fixture consistsof a forbidden region virtual wall/plane to help during a blunt dissection task of a stationary17target on a Zeus robot. A priori data was used to define the fixture.Dynamic 3D virtual fixtures were proposed by Ren et al. [37] to augment their visualguidance system with haptic feedback. Pre-operative MR/CT images are used to generatethe dynamic virtual fixtures, which are then mapped to the patient during surgery. This workcombines two classes of virtual fixtures: guidance and forbidden-region virtual fixtures.The combination of these generates a potential field based fixture that renders anatomicalconstraints sufficiently rapidly to provide meaningful haptic feedback to the surgeon. Thepotential field is based on generalized Gaussian functions and generalized sigmoid func-tions, providing an easily adjusted protective area and continuous force feedback. The val-idation tasks were keeping the tool on the surface of a beating heart, as well as a simulatedcutting/dissection task.Gibo et al. [38] considered the implications of a moving virtual fixture, based on motionof the environment. Two methods of implementation are presented: predicted-position andcurrent-position virtual fixtures. For their experiments, they assumed ideal motion trackingand prediction, and tried 1 DOF simple periodic, complicated periodic, and random motionpaths. The experimental task was to keep the tool tip at a fixed distance beneath the surfaceof the phantom tissue.In more recent work, Navkar et al. [39] demonstrate a guidance approach that usesreal-time Magnetic Resonance Image (MRI) to assist the operator in manoeuvring an inter-ventional tool safely inside the dynamic environment of a the cardiac left ventricle (for aTransApical - Aortic Valve Implant). This is the first work to show real-time guidance onthe beating heart while not using pre-operative information. This is significant because theregistration problem is nontrivial, especially when deformations in the soft tissue are signif-icant. Using real time MRI, the images are processed and a safe access path is calculated,updating a virtual reality scene of the area of operation and driving a force feedback inter-face that is used by the operator for controlling the manipulator. Their experiments wereperformed off-line, but had real, pre-recorded MRI data and a virtual scanner. Their resultsshow that acquiring the MR images is the bottleneck of this process, with the additionalcomplications that interventional tools may distort image.Another recent real-time approach is presented by Ryden et al. [40], though this ap-proach uses RGBD data and streaming point clouds. Without a priori knowledge of the18heart geometry, an efficient method for enforcing forbidden region virtual fixtures is pro-posed by defining a spherical virtual fixtures (with a definable radius) around each pointassociated with the object of interest. The force on the master is calculated as a virtualcoupling between the master position (Haptic Interface Point (HIP)) and the proxy. In theirexperiments, a Phantom device is used for the master, and an Xbox Kinect acquires theRGBD (red-green-blue-depth) data for a beating pig heart, generating a virtual fixture inreal time. The experimental task was to stay a fixed distance from the surface of the heart.The feasibility of having a Kinect in clinical practice is doubtful, and the demonstrated radiifor the spheres in the virtual fixture were in the order of 2 cm, which is an entire order ofmagnitude greater than the dimension of the coronary arteries involved in the anastomosisduring CABG surgery.The main issue in most of these implementations is that they do not demonstrate per-forming complex tasks on the heart surface, for example anastomosis for CABG. The oversimplified tasks demonstrate the advantages of the proposed virtual fixtures, but in a clini-cally practical situation the surgeon will need to penetrate the surface of the heart to performthe anastomosis.19Chapter 3Heart MotionThis chapter will provide a detailed explanation of the heart surface trajectory used forthis work, as well as the technical difficulties of performing motion tracking of the highlydynamic and complex cardiac motion.3.1 Heart TrajectoryThe trajectory of the beating cardiac surface varies by location on the heart, the patient’sage, fitness level, and the severity of coronary disease. As the heart moves from diastole tosystole in the cardiac cycle, the heart walls contract radially and longitudinally, and there isa wringing-like twisting motion of the left ventricle [41]. This corresponds to a complex,deformable motion model for a particular area on the surface of the heart with 3 degrees oftranslation and 3 degrees of rotation. For a given point on the surface, a 3 degree transla-tional trajectory (i.e. rotation may be neglected) may be found, as in [42]. The trajectoryof the heart surface is a function of the cardiac cycle, which is described as quasiperiodic,signifying that it is grossly periodic with slight changes in the amplitude or period lengthbetween periods. Thus, the motion of a point on the heart surface may be represented witha quasiperiodic waveform. It was assumed for this work that the heart motion is periodic.In reality, the heart motion is quasiperiodic, with the potential for arrhythmias. Clinically,beta-blockers can be used to regulate the primary heart frequency.For the work described herein, the waveform reported in [42] was used. In [42], miniatureradiopaque tantalum markers were implanted on the left ventricular midwall of donor hearts20Figure 3.1: Direction of axes of heart motion [43].at the time of cardiac transplantation. Stereo cineradiography allowed measurement of threedimensional coordinates of multiple sites in the left ventricle roughly seven weeks aftersurgery. The coordinate frame for the motion is defined as seen in Figure 3.1. Analyses wereperformed for a point located on the left ventricular mid-wall, where the motion amplitudeis normally the highest. The maximum recorded displacements for this point in the x, y,and z directions were 0.26cm, 0.59cm, and 0.97cm, respectively.For the design of a robust system, worst case constraints must be considered. The trajec-tory was therefore rotated such that the greatest variance was along one axis. This may alsobe thought of as the trajectory which would be perceived from a well positioned robotic arm.Principal Component Analysis (PCA) was used to obtain the rotation matrix which wouldrealign the eigenvectors of the trajectory with the principal axes defined in Figure 3.1. Sincethe z-component was already the greatest in magnitude, the largest variance was alignedwith the z-axis. Further, the reported trajectory only consists of 35 recorded points for oneperiod. To eliminate any artificially high frequencies in the trajectory, a 10 Hz low pass filterwas applied to the motion. This is justified by spectral analyses reported in literature, whichdemonstrate extremely small amplitudes above 10 Hz [11], [12]. Finally, for this work, theheart motion was assumed to be periodic. A comparison of the translation components ofthe original and rotated trajectories may be seen in Figure 3.2. A direct comparison of the210123X [mm]012345Y [mm]0 0.1 0.2 0.3 0.4 0.5 0.602.557.51012Z [mm]Time [sec]  RotatedOrigFigure 3.2: Translation components of original versus rotated heart motion trajecto-ries.Figure 3.3: Comparison of translational components of used heart trajectory.rotated trajectory components is seen in Figure 3.3. Finally, a three dimensional view ofthe rotated trajectory used in this thesis is seen in Figure 3.4. Note that this path is purelytranslational, i.e. for the superimposed control scheme only a translational offset is appliedto the PSM while it is following the automated trajectory. The only rotation commands areissued from the MTMs.22Figure 3.4: 3D view of used heart trajectory.3.2 KinematicsTo calculate the theoretical Cartesian velocities of the heart trajectory, the displacementfor each time step was considered, and divided by a time step corresponding to a heart rate of1.5 Hz, or 90 Beats per Minute (bpm). This number is based on heart motion measurementsand analysis in [33] and [44]. This is again a slight overestimate, but designing for worstcase scenario increases the safety margin of the system for a realistic setting. In clinicalapplications, specific chemicals (beta blockers) may be used to slow down and pace theheart during surgery. Forxi = [xi,yi,zi]Tx˙i = [x˙i, y˙i, z˙i]Tx¨i = [x¨i, y¨i, z¨i]Tthe theoretical Cartesian velocities are calculated usingx˙i =xi−xi−1dt(3.1)23and the resultant theoretical maximum velocities are x˙ = [0.03,−0.12,0.20]m/s. The in-stantaneous acceleration is calculated byx¨i =x˙i− x˙i−1dt(3.2)for small dt, and the theoretical maximum instantaneous accelerations in each dimensionare found to be x¨ = [3.63,10.91,11.96] m/s2.When implemented on the robot, the kinematics of the trajectory are calculated usingthe sensor feedback from each of the joints (more information about the robot is found inChapter 4). The Cartesian position of the end-effector is calculated using forward kine-matics and readings from encoders at each joint in the manipulator. Joint velocities, q˙, arealso provided from the sensor hardware, and Cartesian velocities, x˙ are calculated using theinverse of the manipulator Jacobian, J, followingx˙ = J−1q˙ (3.3)at a specific, centered q.The acceleration is calculated using a second order digital Butterworth filter with a 10Hz cutoff frequency to smooth any noise in the sensor readings. The transfer functioncoefficients were obtained using the Matlab function butter. A plot of the calculatedCartesian kinematics, using the joint encoder readings as input, for several periods of aheart trajectory may be seen in Figure 3.5. It may be observed that the maximum velocityand acceleration values seen in Figure 3.5 are significantly lower than the theoretical valuescalculated above. Recall that the raw heart trajectory data provided in [42] was filtered witha low pass filter with a cutoff frequency of 10 Hz before being commanded to the robot.The theoretical values are calculated using the raw data, but Figure 3.5 uses the filtered data.The reduction of the maximum acceleration verifies the filtering procedure.3.3 Spectral AnalysisFinally, a spectral analysis of the heart motion was performed to evaluate its frequencycomponents and harmonics, shown in Figure 3.6. A discussion of how these harmonics areincorporated in the control design may be found in Chapter 5.24Figure 3.5: Kinematics of heart trajectory.Figure 3.6: Spectral analysis of heart trajectory.25Chapter 4Platform Used / The SystemOne of the primary novelties in this thesis work is the implementation of the heart motioncompensation system on an already clinically prevalent platform, the da Vinci R©SurgicalSystem by Intuitive Surgical Inc. This work was implemented in such a way that the masterinterface is not modified, thus the surgeons’ existing familiarity and experience with therobot may be leveraged. Further, the da Vinci has already received FDA approval and hasbecome the standard of care for certain procedures. Thousands of systems may be found inhospitals worldwide. Its clinical acceptance for soft tissue applications makes it a perfectplatform on which to target more advanced applications. While a complete heart motioncompensation is very complex and would face significant challenges in receiving approvalfrom both the FDA and the community of surgeons, implementing such a system on theda Vinci would mitigate some of these challenges, and increase the feasibility of achievingclinical practice.Despite its advantages, there are still a number of difficulties involved with using such acomplicated robotic platform. This chapter will provide a detailed description of the system,outlining these difficulties, but also discussing some of the benefits of using the da Vinci asa platform for this minimally invasive heart motion compensation system.4.1 The da Vinci Research KitThe dVRK, as discussed in Section 1.2, is an open source mechatronics platform con-nected to components of a da Vinci Classic (the first generation system produced by Intu-26Figure 4.1: The dVRK System at UBCitive Surgical). Developed by a collaborative effort between Intuitive Surgical Inc., JohnsHopkins University and Worcester Polytechnic Institute, the open source platform includes• Mechanical components: robotic manipulators (da Vinci Classic), foot pedal tray,stereo viewer.• Electronic hardware: controller boxes that include circuitry to communicate withthe sensors and actuators in the robotic manipulators, as well as with the computerrunning the robot software [45].• Software: cisst/SAW software libraries that provide access to all levels of control ofthe robot [46].4.1.1 Mechanical ComponentsIn the Robotics and Control Laboratory at the University of British Columbia, the dVRKsetup includes a full da Vinci Classic system, as seen in Figure 4.1. This includes the masterconsole (Figure 4.2) and the Patient Side Manipulators (PSMs) complete with setup joints(Figure 4.3). The master console consists of two MTMs, left and right for bimanual teleop-eration, the stereo viewer for a 3D view of the surgical scene, and a foot pedal tray which27Figure 4.2: The Master Console of the da Vinci ClassicFigure 4.3: The Patient Side Manipulators of the da Vinci Classic28Figure 4.4: Controller boxes for the dVRKallows for digital user inputs to the system. The patient side includes three PSMs and oneEndoscopic Camera Manipulator (ECM), as well as the full setup joints that allow for con-venient positioning of each manipulator. On the PSMs, each joint is independently actuatedwith a Maxon DC motor, and joint position is measured using a rotary potentiometer and acomplementary laser rotary Canon encoder.4.1.2 Electrical HardwareThere is one controller box pertaining to each manipulator, totalling six active controllersin the stack at UBC, seen between the master console and patient side cart in Figure 4.1.Inside each box, shown in Figure 4.4, there are two custom designed IEEE-1394 MotorControllers. Each motor controller consists of a Quad Linear Amplifier (QLA) board andField Programmable Gate Array (FPGA) board. Together, the pair of motor controllersis capable of controlling eight motors, hence one box per manipulator. Each manipulatorconnects to its respective controller box via a zero insertion force DL156 pin connectorand a break-out board provided by Intuitive Surgical. Controllers are connected in a daisy-chain structure using a IEEE-1394a firewire bus that weaves externally and internally to theboxes. A safety chain is also established, allowing for an emergency stop to immediatelyopen the circuit and kill power to the manipulators.The DL156 connector sends current commands from each controller box to the motors,and communicates the sensor readings back to the controllers. Through the IEEE-1394a29chain the sensor readings for each manipulator are sent to the Linux PC performing thehigh level control, where the next motor current commands are computed and sent to thecontrol boxes via the same IEEE-1394a connection.4.1.3 SoftwareThe architecture of the system is designed for centralized computation and distributedI/O, meaning the majority of the control is implemented on a Linux PC and high speedcommunications to the I/O to each of the controller boxes distribute the controls to the me-chanical components. More details about this architecture may be found in Section 5.3. Alow level C++ Application Programmer’s Interface (API) is provided to perform data read-/write through the I/O (writing desired motor currents and reading sensor data using theIEEE-1394a firewire protocol). The most recent revision of the open-source firmware em-ploys the broadcast communication protocol in which asynchronous read/write operationsrequire about 35µs, allowing control rates up to 6 kHz. Finally, a C++ component-basedarchitecture with convenient inter-component interfaces allows for the implementation ofthe joint level servo loop control and high level robot control algorithms on the Linux PC.4.2 Control ArchitectureThe control architecture of the dVRK was developed by Johns Hopkins University, andwas used as the foundation for this work. The automated follow trajectory module was builtdirectly on top of Hopkins’ infrastructure. The control architecture for teleoperation on thedVRK consists of three levels of control implemented in the software modules, and onelevel within the hardware. The lowest level of control resides within the FPGA verilog codewhich performs current control for each of the four motors controlled by a QLA/FPGA pair.Joint velocity estimation and motor current safety checks are also performed at this level.Having this control at the hardware level is the foundation for the centralized computationand distributed I/O architecture.The firmware available on the QLA/FPGA controllers allows for broadcast communica-tion protocols via the IEEE-1349a bus between the hardware and the Linux PC responsiblefor the remaining levels of control. The lowest level of control logic on the software sideruns a joint level servo PID control loop. The software thread on which this control is com-30puted is tied to the I/O thread, minimizing the latency and complications that could arisefrom stale data caused by asynchronous threads. The I/O software modules communicatewith the hardware to read data from the robot sensors and write data to the robot actuatorsdirectly through the IEEE-1394a connection. This information is shared with the softwarelayer containing the joint level servo PID control loops; there is one joint level servo PIDcomponent per manipulator. In this layer, the desired joint positions and current joint po-sitions are used to compute the joint level torques using a PID control scheme, for eachmanipulator. While the first three DOF on each PSM are independent, the last four arecoupled nontrivially, so a joint-to-actuator mapping (instrument specific and provided byIntuitive Surgical) is necessary to convert desired joint torques to actuator torques.The next layer of logic encapsulates the robot arm objects, with one component per MTMor PSM. These components are responsible for computing the forward and inverse kinemat-ics for the manipulator. More details about this are in Section 4.3. At this level, the desiredCartesian position is translated into a desired joint position and sent to the joint level servoPID control layer. Further, this level also includes a gravity compensation mechanism onthe MTMs, implemented using the Recursive Newton Euler Algorithm (RNEA). RNEA de-termines the accelerations and set of joint torques requires to compensate for manipulatordynamics, given the joint positions and velocities. Through system identification experi-ments performed by Omid Mohareri (a past PhD student working on the dVRK), all theMTM dynamic parameters (including link masses and inertia) and some of the Coriolis andcentrifugal parameters for each arm for the da Vinci at UBC were roughly identified andincorporated into the model-based RNEA [47].All these lower levels of control have been defined by the Johns Hopkins group. Thehighest level of control includes the teleoperation and automated trajectory following mod-ules. The trajectory following is my own contribution, built on top of the provided frame-work. The basic teleoperation control structure was established by Hopkins. In the case ofthe motion compensation system, the teleoperation commands are superimposed upon theautomated trajectory being executed by the PSMs. At this layer, the surgeon commandsthe masters to indicate the desired location for the PSM, and this master position is trans-formed to determine the commanded slave Cartesian positions. These desired positions aresent down to the robot arm objects through the component interfaces. The control loopfrequencies for each of the software levels of control for the UBC system were determinedeither by Omid Mohareri or myself, and are described in Table 4.1. Note that the software31Table 4.1: Control loop frequenciesControl Level Loop FrequencyI/O and joint-level PID 1 kHzForward/inverse kinematics 333 HzAutomated trajectory generation 333 HzTeleoperation 200 HzFigure 4.5: Degrees of motion of a PSM [48]is not implemented in hard real time, so the actual frequencies may vary.4.3 Robot KinematicsThe MTMs are 8 DOF actuated manipulators while the PSMs are 7 DOF actuated ma-nipulators. Both are equipped with joint sensors at each joint: a relative, high resolutionlaser rotary Canon encoder and an absolute, lower resolution rotary potentiometer. Theprimary focus of this thesis is to superimpose automated trajectories on the teleoperationcommands for regular use, so while the performance of the MTMs will be the same as inclinical use, the behaviour of the PSMs will be semi-automated. Thus, the kinematic anddynamics properties of the PSMs are of greater importance.The seven joints of the PSM are shown in Figure 4.5. The first three joints are the outeryaw of the arm, outer pitch of the arm, and insertion of the adapter, respectively. The32Figure 4.6: Remote center of motion on a PSM [48]insertion joint is the only prismatic joint, the remaining six are all revolute. The last fourjoints correspond to the outer roll, wrist pitch, wrist yaw 1 and wrist yaw 2, all pertaining tothe dexterous control of the instrument attached to the PSM. The design of the PSM is suchthat it is mechanically constrained to respect a Remote Center of Motion (RCM), as seen inFigure 4.6. This fulcrum point is invariant to the configuration of any of the actuated jointsin the PSM, and may only be relocated by moving the setup joints.To compute the forward and and inverse kinematics of each manipulator, Intuitive Sur-gical has shared a confidential User Guide for the da Vinci Research Kit with the dVRKcommunity. The guide contains the kinematic parameters of the manipulators using theDenavit-Hartenberg (DH) convention. This convention describes how coordinate framesare attached to the kinematic chain of the robotic manipulator, moving from the base tothe tip. It also defines the parameters to determine the transformation between coordinateframes, namely a - the link length, α - the link twist, d - the link offset, and θ - the jointangle. Using this information the forward and inverse kinematics of each manipulator maybe calculated.The coordinate frame of interest is that of the tip of the kinematic chain, located at theend of the end-effector of the attached instrument, O6 in Figure 4.7 just above the jawsof the instrument. The jaws for each instrument are different, so Intuitive selected a moreconsistent location for which to provide the Cartesian position. Also of note in Figure 4.7is the orientation of the coordinate frame at O6. The x-axis corresponds with the outer yaw33Figure 4.7: DH Frames for the PSM and attached (Large Needle Driver) instrument[48]34of the PSM, the y-axis corresponds to the outer pitch, and the z-axis corresponds with theinsertion joint. The axis with the greatest displacement during the heart trajectory is thez-axis, signifying the insertion joint will be under the greatest scrutiny for the analysis ofthe dVRK’s capability of tracking the heart motion. The PSM is a complex, cable-drivenrobotic mechanism with intricate dynamic effects. Specifically, the insertion joint, actu-ated by a cable-driven counter-weight mechanism, demonstrates highly nonlinear frictionbetween its actuated joint limits. This introduces additional complications for controllerdesign, discussed further in Chapter 5.35Chapter 5Controller DesignThis chapter will discuss the design and development process for the controllers em-ployed to try to minimize tracking errors when commanding the dVRK PSMs to track theperiodic motion of a beating heart. For more information about the kinematics and dynam-ics of the PSMs please refer to Chapter 4, and for further details about the heart motionplease look to Chapter 3. As a very quick overview of the heart trajectory, the desired po-sition signal of a point on the heart surface is a three dimensional, quasiperiodic motionrotated using PCA such that the maximum variance occurs along one axis, correspondingto the insertion joint on the PSM. The principal harmonic of the heart motion occurs at 1.5Hz, and the trajectory experiences an amplitude change of nearly 12 mm in the z-direction.For motion tracking, the trajectory has been assumed to be period, and filtered with a lowpass filter with cutoff frequency of 10 Hz.5.1 System IdentificationOne of the primary contributions of this thesis is the use of the da Vinci (specifically thedVRK controlling a da Vinci Classic system) as the platform for a heart motion compensa-tion system. The preliminary research question is whether the da Vinci Classic PSMs areeven capable of tracking the high amplitude, high frequency heart trajectory, or whether itscomplex dynamics and nonlinear friction will prevent it from achieving the required accu-racy. In order to develop a control system for heart motion compensation, the first step isto determine a model, or an analytical representation for the plant to be controlled, usuallyin the form of a transfer function. The transfer function is a representation of a linear time36Figure 5.1: Block diagram representation of the PSM motor plantinvariant dynamical system, describing the relationship between the inputs and outputs ofthe system [49]. The process by which this transfer function is determined is called systemidentification.There are two main types of approaches to performing system identification: first princi-ples and empirical [50]. The first is a theoretical approach which employs fundamental lawsof physics. The latter approach, also called data-driven modelling, is based on analyses ofexperimental observations about the system. The first principles approach requires a verythorough understanding of the physics present in the system, which is less practical for thenonlinear, dynamically complex 7 DOF da Vinci PSM. The data-driven approach is there-fore used here to determine a model for the manipulator. Many techniques are availableto perform the data-driven system identification, including analysis of the step response,frequency response, or impulse response, to name a few. Further considerations includeincorporating the gearing in the drive train, friction modelling, and taking care not to hitany saturation limits – including physical joint limits, velocity or torque limits, etc. – asthis will introduce nonlinearities to the system.As an initial strategy, experiments were performed to try to find the transfer functiondescribing the motors on the PSM, between the joint level torque, τ and joint position, q, asseen in the block diagram in Figure 5.1. The theoretical dynamic equation of a manipulatorcharacterizes this relationship between the torque and manipulator motion. Through theLagrangian equation [51], the following closed-form solution is found to be37Figure 5.2: Block diagram representation of the PSM motor plant controlled by PD(KI = 0).M(q)q¨+C(q, q˙)+G(q) = τ (5.1)where q is the 7×1 vector with the joint angles for the 7 DOF PSM, M(q) is the 7×7 inertiamatrix, symmetric and positive definite for all values of q, C(q, q˙) is the 7× 1 centripetaland Coriolis force vector, G(q) is the 7×1 gravity loading vector and τ is the 7×1 torquevector.Various approaches were considered, including trying to train neural networks to deter-mine the inverse dynamics of the manipulator, or trying to incorporate fundamental equa-tions such as Newton’s equations of angular motion or basic DC motor governing laws.The complexities and nonlinearities in the system made it extremely challenging to obtaina good model. Particularly for the insertion joint, the friction introduced nonlinearity andunrepeatable hysteresis. Commanding a sinusoidal torque to the motor, the drift in the jointposition was clearly evident. The experienced friction was dependent upon on the velocityof the joint, the frequency of the sinusoid, and the joint position, among other factors.The reminder this section describes the process actually used, and an edited descriptionof this process may be found in [52].The plant was therefore redefined to contain the motor plant P as well as a tuned jointlevel Proportional-Derivative (PD) controller, C, with unity feedback, as seen in Figure 5.2.38The closed loop transfer function describing the plant is nowH(s) =CP1+CPAfter tuning the PD following the Ziegler-Nichols method ([53]), tracking errors in theorder of 1-2 mm were achieved between desired and measured Cartesian position. Moreimportantly, the controlled system was now linear, and when sinusoidal inputs were pro-vided to the plant sinusoidal outputs would be observed. To continue with the empiricalsystem identification process, experiments were conducted to gather data about the inputsand outputs of the plant at various frequencies. In future work a nonparametric model couldbe found using Matlab’s tfestimate command.Since the heart motion compensation system is designed to compensate for only thethree dimensional translational components of the heart trajectory, system identificationonly needed to be performed on the first three joints of the PSM. The first three joints areouter yaw (x), outer pitch (y), and insertion (z) and the joints are independent of each other.Note the Cartesian motions are coupled since the instrument has a finite length; in otherwords, if a pure outer yaw motion is commanded the end effector of the instrument willtravel in an arc with varying z-position since the PSM has a fulcrum point at the RCM.For each of the first three joints, the closed-loop frequency response between the com-manded and measured Cartesian positions of the tool tip was found. The generalized formfor the parametric model is a second order transfer functionH(s) =K(1+Tzs)1+2ζTωs+(Tωs)2(5.2)In Equation 5.2, the zero corresponds to the differential term in the PD controller andthe two poles correspond to the inertia and damping which govern the motor dynamics ofthe manipulator. To experimentally determine these parameters, the system was excitedby a position signal composed of a summation of sinusoids. Using a priori knowledgeabout the frequency spectrum of the heart trajectory, the performance of the system wasinvestigated at frequencies corresponding to the spectral frequencies of the heart trajectory.To be consistent with the approach for the spectral line controller (discussed in Section 5.4),the harmonics with the four greatest amplitudes were used, such that the excitation signal39Table 5.1: Experimentally Determined Transfer Function Parameters for PSM JointsK Tz Tω ζOuter Yaw 0.8852 -0.0118 0.0600 0.3291Outer Pitch 0.8917 -0.0150 0.0573 0.2906Insertion 1.0006 -0.0069 0.0100 1.3155for each joint wasx = sin( f1t)+ sin( f2t)+ sin( f3t)+ sin( f4t)f = [1.5,3.0,4.5,7.0]HzThe commanded positions for the other two joints remained fixed at zero. The MatlabSystem Identification Toolbox was employed to find the parameters of the model usingthe provided input-output data. The parameters are summarized in Table 5.1. Each PSMwas thus characterized as a linear combination of three independent Single Input SingleOutput (SISO) plants.The simplifying assumption of a second order system will introduce some error, partic-ularly when the plant is inverted. This assumption is considered acceptable for this fea-sibility study since submillimetre tracking errors were still achieved (see Figure 5.10 andFigure 5.11). In future work, nonparametric data-driven system identification methods willbe employed to eliminate this simplifying assumption.5.2 PID ControlThis section will provide a brief description of one of the most common control schemes,PID control. A block diagram representation of a PID controller is shown in Figure 5.3. Thecontroller accepts as input the tracking error, e(t) which is the difference between the de-sired set point, r(t) and the current measured process variable/output of the plant, y(t). Aseries of calculations are performed using the tracking error to compute the value of thecontrol signal, u(t) to be input to the plant. The control algorithm depends on three in-dependent parameters: the proportional gain, integral gain, and derivative gain, denoted40Figure 5.3: Block diagram of a PID controllerKP, KI , and KD, respectively, in Figure 5.3. Conceptually, the proportional gain tries tominimize the present error, the integral gain considers the accumulation of error, and thederivative tries to prevent future errors. The weighted sum of these three computations de-termines the control input, u(t) for the plant. This controller is widely popular because itrelies purely on the measured output of the plant, not on an understanding of the underlyingphysics and properties of the plant being controlled [54]. Moreover, the common assump-tion is that the plant behaves more or less like a second order (spring-mass-damper) systemin the frequency range in question, which simplifies the modelled plant dynamics.A well-tuned PID controller can be largely successful for a large variety of systems, so theprocess of tuning the controller remains the greatest challenge of PID control. There are anumber of established methods for tuning a controller, such as manual tuning or the Ziegler-Nichols method [53]. Typically more aggressive gains will minimize tracking error and risetime/lag, however the trade off with overshoot, settling time, and most importantly stabilitymust be considered carefully. These considerations vary depending on the application.For this work, there is joint level PID control, meaning the PID parameters for each jointwere optimized. Even so, the heart trajectory is highly dynamic, and attempted experimentsconcluded that tracking errors less than 1.5 mm could not be achieved with a simple PIDcontrolling the dVRK PSM. The vessels involved in the target application of anastomosisfor CABG surgery are around 2 mm in diameter, so submillimetre tracking error is requiredfor clinical feasibility.415.3 Simulink to C++ InterfaceWhile making efforts to design and develop more advanced controllers for heart motiontracking, it was observed that the controller design and development process in the cisst/-SAW framework of the dVRK was quite tedious. The cisst/SAW software architecture forthe dVRK is wonderfully engineered, however it is a huge collection of code written inC++ requiring several minutes to compile. Controller design was being done in the MatlabSimulink environment, utilizing SISOTool and various other useful toolboxes for controlsystem design and analysis. The design would then have to be discretized and implementedin C++, integrated into the cisst/SAW software stack, and then usually debugged. Eachiteration of design was very time consuming. In an attempt to expedite the process, aninterface between Simulink to the C++ cisst/SAW was developed. With this interface, theprocess variables would be communicated from the cisst/SAW code to Simulink, the de-signed control algorithm would compute the process input, specifically joint torque, andthen communicate this value back to cisst/SAW to command the robot. The user couldcontrol whether the default joint level PID control in the cisst/SAW code or the Simulinkcontrol has exclusive command of the robot. Effectively outsourcing the control logic toSimulink, this would allow the control designer to change the controller and test it on therobot without recompiling or power cycling the robot.This work was submitted as an abstract ([55]) and presented as a poster entry at the In-ternational Conference for Robotics and Automation (ICRA) 2015 in Seattle, Washington,at the workshop discussing Shared Frameworks for Medical Robotics Research [1], whereit was the runner up for best poster. The abstract follows in this section.5.3.1 IntroductionThe da Vinci Research Kit (dVRK) is an open source mechatronics platform – consist-ing of hardware, firmware, and software – that serves as a common platform for researchin medical robotics, specifically in the area of telesurgery [46]. The platform has beenspecifically designed to grant researchers access to all levels of control of the robot. A cen-tralized computation and distributed I/O [46] architecture is used to achieve low-latencyinterfaces between the hardware and software, reduce cabling, and allow developers to im-plement their software in a familiar development environment on a high-performance LinuxPC. The PC is connected to I/O hardware (sensor and actuator readings) via an IEEE-139442(FireWire) link layer protocol implemented in an FPGA [56]. Current work is focused onfurther improving this connection with an Ethernet to FireWire Bridge for real-time control[57]. Software for all levels of control is available in a public, open source git repositorycontaining the cisst/SAW libraries detailed in [58].There are currently approximately 17 institutions included in the user group in the dVRKresearch community [59]. These groups study many facets of teleoperation, including hap-tic feedback, semi-autonomous/autonomous control, and gaze tracking. In order to imple-ment a controller required for such projects, the typical procedure for controller develop-ment consists of three major stages: (i) controller design (ii) implementation and (iii) test-ing and debugging. A highly popular tool for controller design is MATLABr Simulinkr,a block diagram environment containing many useful toolboxes and flexibility for mod-elling, simulation, and controller evaluation [60]. Simulink also offers the ability to “peekand poke” at signal values in the model, a highly useful tool in the controller developmentprocess. To implement a controller in the cisst/SAW dVRK platform, a completed con-troller design must first be discretized then integrated into the C++ cisst/SAW software. Fordebugging, user interface components and logging are available for use in the cisst/SAWframework; however, applications for analyzing this data must be written by the researcher.In this work, we propose a novel interface between Simulink and C++ which will allowthe control logic to remain in the Simulink environment for design, easy block diagramimplementation, evaluation, and debugging. With the ability to switch between the nativejoint-level PID controller in the cisst/SAW software and the custom Simulink controller, theresearcher now has the ability to try new controllers “on the fly”. This will serve as a toolfor rapid-prototyping controller designs without having to power off any hardware, writeany C++ code, or compile the large cisst/SAW stack for each modification.5.3.2 Implementation5.3.2.1 Preserving the Component-Based ArchitectureA component-based control architecture is used in the cisst/SAW libraries, with welldefined levels of control and required/provided interfaces between components (shownin black in Figure 5.4). With the aim of making the Simulink to C++ interface abstractenough to be able to connect it at any level of control in the software, a new component,43Figure 5.4: Modified control architecture for one arm using TCP/IP based Simulinkto C++ interface. The logic to switch between controllers is described in indicates the new components and interface, black indicates the originalcontrol architecture in the cisst/SAW framework from [46].mtsSimulinkController, was created. Other components may interface with the mtsSimulinkCon-troller using the standard required/provided interfaces available in cisst/SAW. As a proof ofconcept test, the first Simulink dVRK controller that was implemented mimicked the de-fault joint level PID control. In this example, the mtsSimulinkController block can directlyreplace the mtsPID block, as shown in Figure 5.4.When the mtsSimulinkController is enabled, the mtsPSM component sends the desiredposition information to the mtsSimulinkController via the standard required/provided cis-st/SAW interface. The mtsSimulinkController reads in the current position and velocityinformation from the robot IO, and then packages all the kinematic information into acustom data packet. This packet is then serialized and sent over a TCP/IP socket to theSimulink model. Handshaking is incorporated, and mtsSimulinkController will not sendanother packet until a response has been received. The received packet is accepted overa second TCP/IP socket and contains the joint level torque values which are commandeddirectly to the mtsRobotIO1394 component. As an analogy to the ROS communicationprotocol, mtsSimulinkController is a publisher of the kinematic data and subscriber of thetorque data.445.3.2.2 ThreadingAs an implementation detail, it is noteworthy that for smooth control of the arm, themtsSimulinkController and the IO component must be tied to the same software thread.This is demonstrated between the mtsPID component and the IO component in the open-source cisst/SAW libraries. Two components may be assigned to the same thread using theExecIn/ExecOut interfaces provided in cisst/SAW component code. This is necessary forthe controller components since it guarantees a deterministic order for reading data fromthe robot’s sensors, computing a torque, and writing that torque to the robot actuators. Ifimplemented on different threads, data synchronization issues arise, latent torque values arecommanded, and a significant jitter may be observed in the robot’s trajectory. Communicating with MatlabSimulink 2012b was used for the development of this work. This version of Simulinkdoes offer rudimentary support for TCP/IP communication in the form basic send/receiveblocks. However these are not nearly flexible enough for this application, where customdata packets and serialization are required. As such, specially-designed s-function blockswere generated (Figure 5.5 and Figure 5.6). The s-function block encapsulates a C++ mex-function: a special C++ s-function file was written for each of the send and receive tasks.Each task has unique buffer size and serialization/deserialization requirements. To simplifydata synchronization and timing issues, two TCP/IP sockets are employed, each unidirec-tionally, one strictly for the kinematic data (from C++ to Simulink), and one strictly for thetorque data (from Simulink to C++). In the initialization of the s-functions (performed whenthe Simulink simulation is commenced), the two client sockets (the kinematicDataSocketand the torqueSocket) will connect to two server sockets created in mtsSimulinkController.The initiation of this process is controlled by the user through a check box in the graphicaluser interface. The control will not be passed to the Simulink unless both TCP/IP connec-tions are successful. For every iteration in the simulation,• The msfcn acceptKinematicDataC block will read from the kinematicDataSocket,deserialize the data and parse it into vector outputs for the Simulink model to use(<NumberOfJoints,1> vectors for currentPosition, desiredPosition, and currentVe-locity)• The controller model uses Simulink blocks to take this kinematic data and compute45Figure 5.5: Custom Simulink block for receiving data in Simulink/C++ interface. Thelength of the output vectors can be customized.Figure 5.6: Custom Simulink block for sending data in Simulink/C++ interface.torque is a <NumberOfJoints, 1> vector.the required torque value (<NumberOfJoints,1> vector)• The torque values are serialized and written to the torqueSocket through the msfcn sendTorqueDataCblock. Switching ControlWith the goal of rapid-prototyping, it is necessary to be able to switch to/from theSimulink on the fly. To guarantee safety, the default joint level PID controller is turnedon whenever communication with Simulink is terminated (either due to a communicationerror, the simulation ending, or user-defined termination through the graphical user inter-face). As soon as communication with Simulink is established, the cisst/SAW joint-levelPID is disconnected.465.3.3 AnalysisAs an initial proof of concept, two different controllers were developed. First, the de-fault joint-level PID controller available in cisst/SAW was recreated in a Simulink model(Figure 5.7). Second, a Cartesian controller was developed. To test both, a single slave armwas automated to follow a trajectory. For the dVRK system at the Robotics and ControlsLaboratory at the University of British Columbia (described in detail in [47]), we run thecontrol loop frequencies at 1 kHz for the joint level PID and I/O level control; 333 Hz forthe kinematics and robot logic control, and 200 Hz for the teleoperation control loop forbimanual teleoperation (controlling four robotic arms). Performance of the Simulink in-terface will be considered acceptable if the same frequencies can be maintained while theSimulink controller is commanding the trajectory on the slave arm.Both the cisst/SAW code and MATLAB Simulink were running on the same Linux PC.Table 5.2 summarizes the timing analysis for the experiment from the perspective of the cis-st/SAW code, where µt , t˜, and σt represent the mean, median, and standard deviation of thespecific measurement, respectively. Two measurements for each value are recorded, one forall data, one with the top 5% of the population removed, to eliminate outliers. This table iswritten from the perspective of the cisst/SAW software, so kinematic data is sent, and torquedata is received. The loop time describes a simulation iteration in Simulink; it is measuredas the elapsed time from the moment before the kinematic data is sent to the moment rightafter the corresponding torque data is received. This measure includes the read/write op-eration times over the TCP/IP sockets, serialization/deserialization of the packets (in bothC++ and Simulink), computation of the torque values in Simulink, as well as the time spentwaiting for a handshake confirmation. The sample time for the thread – as controlled by alower level, timer-based component manager in the cisst/SAW code – is listed at the bottomof the table. These results demonstrate that the communication and processing offloadedto Simulink take only a small portion of the 1 msec period allotted for the IO and controls.The 1 kHz control loop frequency for both the joint level PID and Cartesian controller wassuccessfully achieved when interfacing with the Simulink controller.Occasionally during the experiments it was observed that Simulink would irregularly andtemporarily experience a brief delay. Besides the robustification provided by the handshak-ing, additional safety features have been included to automatically switch back to the defaultmtsPID controller if the communication interruption is too long. The exact cause for these47Figure 5.7: Joint-level PID for a slave arm on the dVRK implemented as a Simulinkmodel. The TCP/IP Receive Data subsystem on the left contains the block inFigure 5.5 and reroutes the signals to 7 <3,1> vectors, one for each joint. TheTCP/IP Send Torque subsystem on the right consolidates the 7 torque values intoa single vector and feeds this into the block in Figure 5.6.48Table 5.2: Timing analysis for Simulink joint-level PID (PIDθ ) and Cartesian (PIDx)controllersMeasurement PIDθ PIDθ (95%) PIDx PIDx (95%)Bytes per sent packet 326 - 452 -Sending data µt [msec] 0.0379 0.0306 0.0379 0.0318t˜ [msec] 0.0283 0.0282 0.0317 0.0315σt [msec] 0.0391 0.0081 0.0357 0.0053Bytes per received packet 69 - 54 -Reading data µt [msec] 0.0022 0.0016 0.0013 0.0008t˜ [msec] 0.0011 0.0011 0.0008 0.0007σt [msec] 0.0027 0.0020 0.0025 0.0002Loop time µt [msec] 0.1514 0.1401 0.1867 0.1768t˜ [msec] 0.1395 0.1391 0.1845 0.1817σt [msec] 0.0816 0.0274 0.0698 0.0254Sample time µt [msec] 1.0631 1.0516 1.0626 1.0526t˜ [msec] 1.0559 1.0552 1.0561 1.0553σt [msec] 0.1205 0.0402 0.1138 0.0335delays is still under investigation, but it is suspected that having a PC dedicated to runningthe Simulink controller, instead of having one PC running both Simulink and the cisst/SAWapplication, would help alleviate this issue. Another potential cause of the delays is that theLinux PC used for the experiment was a non-hard real-time operating system.A potential alternative solution for the interface would be to integrate UDP instead ofTCP/IP sockets. Many resources discuss the comparison between these protocols. Ul-timately, TCP/IP was selected for its reliability. While the extra handshaking betweenmtsSimulinkController and Simulink adds additional robustness, the data integrity guaran-teed by TCP/IP transmission was deemed beneficial for this surgical robotics application.Also, UDP is a connectionless protocol; communication is datagram oriented. This would49make it more challenging to do safety monitoring of the integrity of the C++/Simulink in-terface. Instead, using TCP/IP, the connection status of the communication is immediatelyknown, resulting in faster responses to error conditions. The most significant disadvantagewith TCP/IP is that it is generally slower than UDP, but our experiment demonstrates thatits performance is sufficiently fast for this application.5.3.4 ConclusionThe Simulink to C++ interface described in this paper is a handy tool for controller devel-opment on the dVRK. This interface allows researchers to design, implement, and debugcontrollers all in the friendly MATLAB Simulink block diagram environment. Custom-written s-function blocks perform the tailored TCP/IP read/write functions. The Simulinkcontrollers can be easily modified on the fly, and then be used to command the robot with-out having to recompile or relaunch the cisst/SAW code. The user specifies through a userinterface when to switch control to/from the Simulink controller, and a safety switchingmechanism will automatically switch to the default joint-level PID controller whenever theSimulink controller terminates. Further, the ability to “peek and poke” at signal values inthe Simulink environment and monitor, store, and evaluate these signal values on the fly isa very helpful debugging tool. These advantages aim to simplify and expedite controllerdesign for the dVRK. Once the controller design is completed, Simulink offers a C++ codegeneration tool which will transform the Simulink block model into C++ code which canthen be easily integrated into the native cisst/SAW code.Due to its modular nature, the general TCP/IP interface to/from Simulink may also beeasily modified to communicate with other robotic platforms. This work will be packagedand prepared for distribution; its desired intent is to provide a controller development toolfor the robotics community. The package will include the joint level PID and Cartesian con-troller examples. Please contact the corresponding author for details about the distribution.5.4 Spectral Line ControllerBoth this and the following sections have been previously described, in less detail, in[52].50As described in Section 5.2, the PID controller is capable of achieve good tracking re-sults for following the heart motion, however better performance is required to achievesubmillimetre tracking errors. For a more advanced controller, the quasiperiodicity of theheart trajectory is leveraged. (The spectrum and heart trajectory may be found in Chap-ter 3.) This work investigates the development of a spectral line controller to track theheart motion. The fundamental idea of the spectral line controller is to use the inverse ofthe transfer function at the heart motion spectral frequencies to compute a reference inputthat minimizes the tracking error. The published heart trajectory is used as prior knowledgeto optimize the motion of the da Vinci PSM.While spectral decomposition has been used before for periodic trajectory control [13],and proposed for heart motion tracking [61], it has not been used before for controllinga robot to follow heart trajectories. To accurately follow the heart trajectory, the Fourierspectrum of the target trajectory – the heart motion with a fundamental frequency of 1.5 Hz(consistent with [11] and [13]) – must be analyzed. The first step is to find the critical spec-tral lines and rank them by their amplitude. The frequency spectrum of the heart trajectoryis shown in Figure 3.6 for reference, and Table 5.3 shows the extracted ranked harmonics.The design tradeoff for the spectral line controller is between the complexity of the con-troller and performance. A greater complexity is introduced with a greater number of spec-tral lines. To determine how many spectral lines are needed for an adequate representationof the heart motion, the motion is reconstructed using the inverse Fourier transform of theheart trajectory was taken using the top N = [1,10] spectral lines. Figure 5.8 shows the totalerror over one period of the heart motion between the reconstructed trajectory and the orig-inal trajectory, depending on the number of spectral lines used for the reconstruction. It wasTable 5.3: Ranked harmonics of the heart motionHarmonic Frequency Amplitude [x,y,z]1 1.5 Hz [52.18, 37.43, 1798.40]2 3.0 Hz [32.83, 212.20, 216.20]3 4.5 Hz [63.06, 178.31, 172.63]5 7.5 Hz [12.28, 84.79, 13.99]4 6.0 Hz [21.29, 67.23, 81.12]6 9.0 Hz [13.63, 22.72, 12.28]7 10.5 Hz [ 2.41, 4.48, 4.16]511 2 3 4 5 6 7 8 9 10024681012141618Number of Spectral linesTotal Error over One Period [mm]  xyzFigure 5.8: Evaluation of heart motion reconstruction (total error over one period)versus number of spectral linesFigure 5.9: Difference between original and reconstructed heart trajectories using N =4 spectral linesdecided that using four spectral lines would provide a reasonable performance-complexitytradeoff, so the frequencies of [1.5, 3.0, 4.5, 7.5] Hz were used further in this study. Thedifference between the original and reconstructed trajectory using N = 4 spectral lines canbe seen in Figure 5.9.52To determine the pre-conditioned input trajectory that will minimize the tracking errorwith the spectral line controller, the inversion principle for a linear feedforward controllerdesign is employed. The analytical formulation for the input is as follows:ud(t) = R{N∑k=1H( jωk)−1G( jωk)e jωkt} (5.3)where G( jωk) is the Fourier series of the heart motion at the k = 1, ...,N spectral frequen-cies, and H( jωk) is the closed loop frequency response obtained in the system identificationprocess described in Section 5.1. At each of the N harmonic frequencies, ωk, the closed-loop transfer function is inverted and multiplied by the corresponding harmonic of the heartsignal. The results are summed over the N frequencies, then the inverse Fourier transformis used to obtained desired trajectory input ud in Cartesian space along one dimension. Thismust be repeated for each dimension of each PSM used in the heart motion compensationsystem.This pre-compensator worked well on the da Vinci PSM; the tracking results are summa-rized in Table 5.4 and Figure 5.10 and Figure 5.11. The first 0.2 seconds of initializationwere not considered for the analysis. These results demonstrate that a heart motion track-ing system on the da Vinci is feasible, verifying that the system is mechanically capable ofachieving the dynamics required. Further, assuming the heart trajectory is periodic, track-ing in open loop using this method can be sufficient if adjustments are made to the spectralline coefficients using a Kalman filter, as suggested in [61] for heart motion tracking, orother approaches.From these results it was observed that a frequency component at 6.0 Hz was clearlyevident in the tracking error signals, so incorporating this frequency into the computationof the pre-conditioned input would likely further reduce the tracking error. The greatesterror is present in the y dimension, which corresponds to the outer pitch joint. During theTable 5.4: Tracking Errors using Spectral Line ControllerX [mm] Y [mm] Z [mm] Total Euclidean [mm]Max. Error 0.1882 0.5863 0.3859 0.7267RMS Error 0.1147 0.3154 0.1090 0.3529530 0.5 1 1.5 2 2.5 3 3.500.51x 10−3X−Position [m]0 0.5 1 1.5 2 2.5 3 3.5024x 10−3Y−Position [m]0 0.5 1 1.5 2 2.5 3 3.5−0.095−0.09−0.085Z−Position [m]Time [sec]  DesiredMeasuredFigure 5.10: Commanded and measured trajectories over five periods0 0.5 1 1.5 2 2.5 3 3.5−2024x 10−4Error−X [m]0 0.5 1 1.5 2 2.5 3 3.5−101x 10−3Error−Y [m]0 0.5 1 1.5 2 2.5 3 3.5−4−202x 10−4Error−Z [m]Time [sec]Figure 5.11: Tracking error for heart motion compensation over five periodssystem identification stage, when performing the data-driven modelling to fit the parametricsecond order transfer function, a measure of the quality of fit was provided. The measurefor the outer pitch was less than for the outer yaw and insertion joints, which matches thetracking error results. Due to the sensitivity of the inversion principle, if the model of theplant is poor, i.e. the poles and zeros of the plant are inaccurately positioned, the resultanttracking performance may be made worse. Instead of cancelling out the effect of the plantdynamics through inversion, additional dynamics may be introduced which may be evenmore challenging to control. Moving towards a non-parametric model of the joints (using54data driven modelling technique and Matlab’s non-parametric system identification tools,for example) would eliminate some assumptions about the plant and might reduce some ofthe error in the y dimension.5.5 Visual StabilizationThis work proposes a novel approach to virtual stabilization of the heart by decouplingthe motion measurement and motion tracking tasks that are required in an actual system.A perfect motion measurement environment is simulated by mounting the camera in fixedrelationship to the phantom that simulates the heart motion.To provide a stable image to the console in a virtually stabilized environment, the cameramust track the heart motion accurately either physically, through actual camera motion, orvirtually, with 3D tracking and stabilized rendering. For preliminary studies, we proposehere to decouple the visual tracking problem from the manipulation tracking problem bymounting a camera at a fixed distance to the payload that simulates the beating heart. Thus,for the study herein a small, light stereo camera was mounted to the robotic arm carryingthe simulated heart target using a 3D printed fixture (seen in Figure 6.1 or Figure 6.2). Ini-tially a prototype for the single-port da Vinci system, this camera (shown in Figure 5.12)is lightweight (about 8 g) with a small form-factor and provides standard definition stereo-scopic views of the surgical site. This camera was interfaced to the da Vinci Classic masterconsole so the surgeon is able to watch the camera feed in the actual stereo viewer in arealistic setting.Figure 5.13 shows how the camera was mounted to achieve visual stabilization. Initially,for the proof of concept system, the camera was mounted with 3D printed plastic compo-Figure 5.12: Camera and 3D printed housing55Figure 5.13: Camera mounts for visual stabilizationnents directly to the instrument shaft, at a fixed distance from the heart target which wasalso mounted to the same shaft. In the second generation system, which included motionmeasurement, seen on the right, the camera is mounted to the tip of an instrument with a3D printed plastic component again.56Chapter 6Initial User StudiesWith the system identification and spectral line controller design complete, and with thefollow trajectory software module implemented, the system was ready to be evaluated bysurgeons. User studies were performed to verify the initial, proof-of-concept system, withvery promising results. A description of the system and user studies was submitted andaccepted as a conference paper to ICRA 2015 [52], and has been described in further detailin the previous chapters. This system was also entered in the first Surgical Robot Challengewhich took place at the 2015 Hamlyn Symposium for Medical Robotics, and the entry videomay be found at [62]. This chapter will discuss the user studies performed to evaluate theproof of concept system.As a quick overview, the system approach being evaluated consists of (i) using the aux-iliary PSM to move a payload that simulates the heart surface motion, (ii) implementingtracking controllers to have the left and right PSMs track the payload, and (iii) demonstrat-ing through standardized skill assessment tasks that teleoperation relative to the payloadcan be carried out without compromising task time and quality.6.1 User StudiesThe goal of the user study was to determine the change in task quality and completiontime, when carried out by our motion compensating system on a payload simulating thebeating heart, relative to the gold standard, which is the same task carried out on a stationarypayload.576.1.1 SetupThe participants sat at the da Vinci Classic master console and could see the stereoscopiccamera feed in the native 3D viewer. They could interface naturally with the MTMs andclutch foot pedal to modify their workspace. Their teleoperation commands were super-imposed upon the automated trajectories. At the slave manipulators, one PSM carried atarget which simulated the heart surface while the other (right) arm performed the task.The heart surface targets were 3D printed fixtures mounted directly onto the shaft of the daVinci large needle driver. A similar clamping mechanism was used to secure the camera tothe same tool shaft above the platform. The pitch of the camera could be adjusted using asimple pin mechanism. The view of the camera could thus be adjusted with three degreesof freedom: height, roll around the tool shaft, and pitch angle. Two LEDs were mounteddirectly beneath the camera to help illuminate the scene (see Figure 6.1 - upper white partholds camera and LEDs).Two different benchmark tasks were completed, each under the following five conditionscompleted in the same order:• C1: everything stationary• C2: moving target, no compensation• C3: moving target, compensation enabled• C4: moving target, compensation enabled with smaller amplitude motion• C5: moving target, compensation enabled with slower frequency of motionCondition 1 was designed as the control, analogous to robotic surgery on the stopped heart.Condition 2 simulates having to perform surgery on the beating heart, without any motioncompensation assisting in the task. Condition 3 is the desired use case for full frequency,full amplitude compensation. Conditions 4 and 5 simulate potential solutions which can beemployed should condition 3 be too difficult: smaller amplitude motion may be achievedusing mechanical stabilizers, whereas condition 5 may be achieved using beta-blockers andheart pacing. Both solutions are clinically acceptable. For all the conditions, the camerawas mounted at a fixed distance from the heart target, offering a visually stabilized viewwhen the target was moving.586.1.2 Participants and experiment completionFive right-handed participants were included in the study: three cardiovascular surgicalresidents with basic da Vinci training, and two surgeons who are expert da Vinci users. Oneof the expert users is the only cardiac surgeon in central and western Canada to performrobotically assisted cardiac surgeries.The users were given time to practice, and then three trials of each condition were exe-cuted. For conditions 2 through 5, the da Vinci slave arm(s) was(were) automated to followa realistic pre-programmed heart trajectory. The virtually stabilized field was observed inconditions 3 through 5. The surgeon’s commands through the MTM were superimposedupon the automated trajectory of the right-hand arm. For conditions 1 and 2, the right slavearm performed the surgeon’s commands exactly, whereas for conditions 3 to 5 the surgeon’smotions were augmented with the heart trajectory. After completing all conditions for onetask, the participants filled out a questionnaire evaluating their subjective perception of thedifficulty of each condition for each task.6.1.3 Task DesignThe two benchmark, one-handed tasks that were selected for this study include a sim-ulated suture and a pegboard transfer task. The simulated suture task fixture is shown inFigure 6.1. Following [10], paper targets with dot patterns were affixed to a 3D printedplastic frame such that the surgeon was able to pierce the paper with a needle at the printeddot locations. Working towards the ultimate goal of anastomosis, the first step is to verifythat the surgeon can place the suture at the desired locations with sufficient precision. Thedots had a diameter of 1 mm and the needle’s diameter was 0.65 mm. A target was “hit” ifthe error was <0.5 mm, and “missed” if the error was ≥1.0 mm. Successful completion ofthis task requires high accuracy, similar to what would be expected for actual anastomosisof a coronary artery. The users were instructed to pierce the dots one pair at a time, with astraight needle, simply perforating each printed target from above each time, i.e. the needlewas not passed underneath the surface of the target. Each trial consisted of six pairs, i.e.twelve total piercings. For this task, the performance was evaluated by time of completion,accuracy of the piercing, as well as number of mistakes (e.g. multiple perforations of thepaper).59Figure 6.1: Task 1 : Simulated sutureThe second task was a pegboard transfer task based on the laparoscopic skills trainingcurriculum from the Fundamentals of Laparoscopic Surgery program. This psychomotortask has been validated as a standard test of a surgeon’s efficiency and precision in a laparo-scopic environment [63]. The user was asked to pick up rubber O-rings from a bin and placethem on top of pegs (see Figure 6.2). Again this platform was 3D printed from lightweightplastic. Various sized rings and pegs were available, but each user was instructed to com-plete the same configuration of ring/peg pairings. The user was asked to place two smallrings (internal diameter 3 mm) on the smallest pegs (outer diameter of 1.5 mm), and fourlarge rings (internal diameter 6 mm) on the largest pegs (outer diameter of 3 mm). Thus thistask had a slightly larger margin for error. User performance for this task was measured bytime of completion and number of errors (e.g. dropped rings, missed pegs).6.1.4 Results and DiscussionIn both tasks, the user was evaluated in terms of their efficiency (measured in time), andtheir accuracy. For the suture test, the accuracy was determined by the distance of the needlepiercing to the center of the printed dot; for the pegboard task, the accuracy was measuredin the number of mistakes, i.e. how many times a peg was missed or a ring was dropped.The results are shown in Figure 6.3 and Figure 6.6.60Figure 6.2: Task 2 : Peg and ring task1 2 3 4 5050100Time [sec]1 2 3 4 500.51Average      Accuracy [mm]  C1C2C3C4C51 2 3 4 50510Number of    Hit TargetsUser 1 2 3 4 502.55Number of     Missed TargetsUserFigure 6.3: Results of Suture Task. Top-left: Average time of completion. Top-right:Average accuracy/distance from center of target. Bottom-left: Number of targetshit with an error of <0.5 mm. Bottom-right: Number of targets missed with anerror of ≥1.0 mm6130 40 50 60 70 80 90 10000. [sec]Average Accuracy [mm]  C1C2C3C4C5Figure 6.4: Tradeoff between completion time and accuracy for suture taskThe suture task was the more technically challenging task for the participants, but thehigh precision required to complete the task made it a very good indicator of the effects ofthe various conditions on the participants’ performance. As can be seen in Figure 6.3, withthe exception of user 2, the efficiency (performance time) deteriorated between conditions1 and 2. The average accuracy also worsened, nearly doubling for most users when thetarget moved without compensation. However, once the compensation was enabled, thecompletion time was reduced slightly, and there was a significant improvement in accu-racy. For most users the completion time with compensation enabled was slower than thestationary conditions, and the accuracy was also poorer. The effects of conditions 4 and 5,reduced amplitude of motion and reduced frequency of motion, showed varying results withthe users. The users’ subjective evaluations, communicated in discussion after the tests, re-flected these differences. Figure 6.4 shows the performance tradeoff between completiontime and accuracy achieved for the various conditions. Generally a better accuracy (lowererror) is obtained with longer completion times. The varying skill levels of the users is alsodemonstrated in this plot.The bottom row of charts in Figure 6.3 shows a count of the average number of targetsthat were pierced within 0.5 mm as well as past 1.0 mm of the center of the target duringthe suture task. Each trial consists of piercing twelve targets. Figure 6.5 shows a summaryof the results averaged across all the participants. These results demonstrate that there is asignificant reduction in accuracy when the target is moving, however a lot of this accuracy is62204060801 2 3 4 5Percent Hit Targets02040601 2 3 4 5Percent Missed Targets−50−25025501 2 3 4 5ConditionPercent Change in TimeFigure 6.5: Summary of inter-user results for accuracy and completion time for suturetask1 2 3 4 5020406080100Time [sec]User 1 2 3 4 500.511.52Number of MistakesUser  C1C2C3C4C5Figure 6.6: Results of Peg Task. Left: Average time of completion. Right: Averagenumber of mistakesrecovered once compensation is enabled. Figure 6.5 also shows a summary of the change incompletion time, relative to time to complete the stationary task, for each task. Comparingspecifically conditions 2 and 3, the number of missed targets decreased from 37% to 13%when compensation was enabled, the number of high accuracy hits increased from 26% to41%. The motion-compensated results are still not as successful as the stationary condition,but these are promising initial results.63The pegboard task was decidedly an easier task for users. This was reflected both in thequantitative results and the qualitative evaluations provided by the participants. Figure 6.6shows a decrease in efficiency (increase in task completion time) between conditions 1 and2, and a significant increase in the number of mistakes. With motion compensation enabled,the effect on time of completion and number of mistakes varied between users. It is expectedthat due to the low level of difficulty of this task, these results may be attributed to randomfactors and not necessarily the test conditions. Subjectively, all users felt that condition 3was easier to complete. The reduction in amplitude and frequency seemed to have a morepronounced effect on the results in the pegboard task, showing inconsistent effects in thenumber of mistakes, but showing a decrease in completion time from condition 3. Despitethe fact that a practice period was provided at the beginning of the test, it is expected theseconclusions may be affected by natural human learning.Further, it was observed in the user studies that an element of natural compensation wasperformed by the human user. Using the knowledge that the trajectory is periodic, theusers were able to consciously predict the best location and best time at which to make amovement. This was clearly evident in the uncompensated trials, yet the results show thelimits of what human compensation can achieve. With robotic compensation, the amountof human compensation that must be performed by the surgeon is reduced, and results inbetter accuracy and faster time of completion for these preliminary tests.Statistical analysis using a two-tailed Student’s t-test was conducted, however the resultsdid not yield statistical significance. This may be attributed to the small sample size incombination with the variance in their approaches to completing the tasks. Regardless, theresults look promising enough to encourage future work. Two of the five users were expertrobot users (robotic surgeons) for these experiments, and user studies with larger samplesizes will be conducted pending future hardware improvements.We are aware that a number of limitations are present in our study. Our test setup requiresfurther characterization and improvements. The heart trajectory that we use was determinedfrom the da Vinci robot kinematics and has not yet been verified independently. We willcarry out such characterization using an NDI Certus system in the near future. The cameraused in the user studies uses the NTSC standard, and provides the surgeons with imagingresolution that is below what they are used to in their clinical systems. Further, we realizedthat our camera implementation for condition 2, with the camera fixed relative to the moving64platform, might be more difficult than a more realistic situation of a globally fixed camera.In our implementation only the instrument was moving relative to the camera, giving theillusion that the tool is in motion even though the surgeon’s hands are fixed; whereas witha fixed camera, the instrument would correctly appear stationary and only the target wouldbe moving. Also, after data analysis, we also realized that the peg transfer task is notsufficiently challenging as implemented, but for future studies this could be easily adjustedto be more difficult. Finally, the most prevalent task in CABG surgery is anastomosis, butthis was not included in our study.6.2 Future WorkAs a preliminary study in developing a clinically viable heart motion compensation sys-tem on the da Vinci surgical system, initial results and feedback from surgeons are promis-ing. Future work will start by addressing the shortcomings mentioned in Section III.D. Thepeg transfer task will be made more challenging by increasing the peg size and reducingthe ring size to leave a smaller margin for error. We will add a test condition similar to con-dition 2 but in which the camera is stationary. We will develop a similar tracking systemfor the da Vinci camera arm, which has different kinematics and dynamics than the manip-ulating PSMs. Once the camera can follow the heart trajectory, we will be able to studythe effect of heart motion sensing delay (due, for example, to image processing for visualtissue tracking) on task completion and quality. Our testing conditions will be expandedaccordingly. Anastomosis tasks will be added to our user studies with a suitable test ofanastomosis quality and patency.For clinical implementation, a heart-tracking system will require a closed loop controller,not an open-loop pre-compensator as used in our tests. We will start closed loop controlby extending our spectral line approach, with the use of in-loop harmonic oscillators thatprovide infinite gain at the heartbeat spectral frequencies. We will consider adaptation andlearning options to improve performance.Understanding how users adapt to operating on a moving target is another interesting areaof research that can be carried out by altering the tracking controller performance. In futureuser studies, human performance can be monitored while modifying the tracking delays ortracking errors, to determine what inaccuracies may be tolerated while still achieving the65required accuracy for anastomosis.In parallel to controller development, work towards sensing heart wall motion robustlywill continue. This area of research is an exciting visual servoing problem as three da Vinciarms would have to follow the heart motion in order to perform accurate surgery on thebeating heart.6.3 ConclusionsIn this work we presented a novel application of the da Vinci surgical robot for heartmotion compensation. We showed that the da Vinci Classic PSMs are capable of trackingan actual heart trajectory with small errors. To achieve these errors we used a simple ap-proach based on the spectral decomposition of the desired trajectory. Simulating a virtuallystabilized field, we evaluated the ability of surgeons to perform tasks on a moving, simu-lated heart target. All users reported the highest perceived difficulty for the condition of amoving target with no compensation. This was supported by slower completion times inboth tasks, poorer accuracy and greater number of mistakes and additional perforations inthe case of the suture task. When the motion compensation was enabled, the users felt areduction in the difficulty of task completion. While their completion times were longer andhad poorer accuracy than the gold standard of a stationary target, the loss in these measureswas moderate. Improvement was observed between the cases of disabled and enabled com-pensation on the moving target, both objectively in terms of accuracy and completion timeand subjectively in terms of perceived challenge. This improvement was achieved despiteimperfections in the test setup and slight delays in the tracking. It is anticipated that withfurther development of the present system, performance comparable to the gold standardcould be achieved for benchmark tasks. Further work is warranted to study anastomosistasks under conditions similar to surgery.66Chapter 7Virtual FixturesThis chapter will discuss an addition to the heart motion compensation system pertainingto including virtual fixtures, or active constraints. It was observed during the user studythat minor latency errors were present in the system, and for this reason and perhaps dueto the virtual stabilization, the user would accidentally perforate the heart surface as it wasmoving upwards towards the tool tip. As an added a safety measure, the efficacy of a mov-ing wall virtual fixture to prevent accidental collisions was evaluated. To reduce the amountof obstruction to the surgeon’s desired task, the virtual fixture is also designed to only be ac-tivated as the heart surface is accelerating upwards towards the surgical instrument, deemedthe “high risk” portion of the trajectory. Instead of forbidding the surgeon from collidingwith the heart surface (which is clinically infeasible; to complete the anastomosis the sur-geon must penetrate the heart surface), this virtual fixture is meant to guide the surgeon toperform the majority of the instrument displacement during the “low risk” periods of theheart trajectory.During a Visiting Student term at the Collaborative Haptics and Robotics in MedicineLaboratory in the Mechanical Engineering Department at Stanford University, this virtualfixture was developed and tested with user studies. The beauty of the dVRK, a shared med-ical research framework, is well demonstrated in this work since the heart motion compen-sation system from the University of British Columbia was ported over onto the Stanfordsystem in a manner of hours on the software level. The results of the user studies werepublished in the proceedings for the 2015 Hamlyn Symposium on Medical Robotics [64].The abstract follows in the remainder of this chapter.677.1 IntroductionVirtual fixtures, or active constraints, are a controls concept that enable robots to intelli-gently collaborate with human users to complete tasks that are too complex for the robot tocomplete autonomously and too challenging for the user to complete unassisted. The bodyof work on dynamic active constraints is not extensive, but combinations of guidance andforbidden-region dynamic virtual fixtures for teleoperation have been studied [65]. Fixtureevaluation methods in previous work include potential fields determined from pre-operativeimages, predicted and current positions of the environment surface location, and streamingpoint clouds. Some of these do not allow the slave to penetrate the forbidden region, whichis impractical in surgical situations. This work proposes an impedance-controlled regional3D dynamic virtual fixture generated based on dynamics of the heart motion. This virtualfixture has been added to the heart motion compensation system implemented on the dVRKdescribed in [52]. In this work it was noted that while a user was operating on a virtually sta-bilized heart, the dynamics of the heart target were not accurately perceived. This resultedin additional, inadvertent perforations. To augment the surgeons perception, we propose anovel non-continuous, strobe-like virtual fixture whose enforcement is conditional upon thekinematics of a beating hearts trajectory.7.2 Materials and Methods7.2.1 Virtual Stabilization System OverviewThe heart motion compensation system from [52] was implemented on the dVRK at theCollaborative Haptics and Robotics in Medicine Laboratory at Stanford University, seen inFigure 7.1. In the heart motion compensation system, the PSMs are programmed to followthe trajectory of a point on the heart surface, with superimposed teleoperation commandsissued from the MTMs. This results in a virtual stabilization of the heart motion from theperspective of the surgeon. The 3D printed plastic heart target is controlled by one PSMwhile the surgeon controls the other PSM. The camera is attached to the plastic target ata fixed location relative to the simulated heart surface, emulating perfect camera motiontracking.68Figure 7.1: dVRK setup for virtual fixture experiments for beating heart applicationsat Stanford University.Figure 7.2: High risk (blue) versus low risk (green) periods of the heart trajectory.697.2.2 Virtual Fixture DesignIn previous user studies for the heart motion compensation, it was difficult to see whenthe heart surface is accelerating upwards. This strobe fixture thus provides pulsing hapticfeedback, active during periods of high acceleration in the heart trajectory. A 2nd orderButterworth Filter is used to calculate the z-acceleration, az, of the heart using joint velocityreadings from the position sensors in the PSMs. It was experimentally determined that, forthe trajectory (as in [52]) with a z-displacement of 11 mm at 1.5 Hz, the virtual fixturestrobe pulse commences when az > 0.39m/s2 and terminates when acceleration falls belowaz < −0.145m/s2, portrayed in Figure 7.2. The force feedback on the master is displayedonly in the positive z direction. The magnitude of the feedback is calculated using therelative position of the slave tool tip and a proxy following the heart surface (similar to theconcept in [66]). The tool tip and proxy follow an elastic linkage model:δz = zheart − ztooltip (7.1)FMT Mz =V Fscale× (δz +V Fmargin), if δz > 0,0 otherwise.(7.2)(7.3)A virtual margin of 5 mm is added to the surface of the heart so the imperceptibly lowFMT Mz occurs before collision between the tool tip and the heart surface. The dynamicsand inertia of the MTMs guarantee a smooth transition across the FMT Mz = 0 threshold.V Fscale ≈ 150; δz is usually only a few millimetres so large scaling values are needed togenerate perceptible haptic feedback. The torque applied to the MTM motors to display thehaptic feedback is clipped to 1.0 Nm to avoid instabilities. The virtual fixture control loopruns at a frequency of 1 kHz.7.3 User StudiesSeven right-handed subjects completed two one-handed tasks in an IRB-approved studywith informed consent: drawing a circle (relevant to the standard Fundamentals of La-paroscopy circle cutting task), and a simulated suturing task (piercing printed targets with a70Figure 7.3: Tasks completed for virtual fixture user studies. Left: drawing a circle.Right: simulated suturing.needle) (Figure 7.3). Heart motion compensation was active for both.For drawing a circle, a 3D printed adapter was used to attach the marker coaxially to theinstrument shaft. For suturing, a straight needle and 6 pairs of dots were used. Each dotis a circle 1 mm in diameter and the needle is 0.5 mm in diameter; high accuracy (similarto actual anastomosis) is required for this task. A target is “hit” if the needle perforationis completely within the printed dot, “missed” if the perforation is completely outside theprinted dot, and “almost” otherwise. The subjects had varying levels of expertise with therobot and haptic devices. Each task was performed under four conditions; the subjectscompleted practice and three test trials for each. The conditions were• C1: no feedback• C2: basic moving wall virtual fixture: feedback is calculated using the elastic linkagemodel (described in Section 7.2.2). This has been implemented in previous work [36],and is the baseline for comparing the effect of the non-continuity of the proposedvirtual fixture• C3: haptic pulse feedback: the pulse is active during high acceleration periods, irre-spective of the relative positions of the tool tip and heart surface• C4: pulsing moving wall virtual fixture: based off C2 with additional dependencieson the dynamics of the target717.4 ResultsThe performance of the subjects under the various conditions was evaluated based onfour indicators:• Efficiency: time of completion• Accuracy: how many times the marker tip left the surface (circle task); how manyhit/missed/almost targets (suture task)• Damage: the collision extent or time under the surface multiplied by the distanceunder the surface• Safer Motion: to evaluate the efficacy of the non-continuity, we compare the motionof the teleoperated slave during the high-risk periods of the trajectory to the motionduring lower-risk periods.The results shown in Figure 7.4 (except for the circle results in row II) depict the averageover the seven users. Efficiency and accuracy measures are normalized relative to the resultsfor C1, showing the effect of the feedback.7.5 DiscussionBased on the efficiency results, C2 was comparable to C4. However in the accuracyresults for the circle task, C4 was the only condition with improved performance over C1.The collision extent results show the benefits of virtual fixtures, where all conditions withhaptic feedback surpassed the results from C1. The ranking of the feedback conditionsvaried depending on the task. The advantage of the non-continuity is seen in the safe motionresults, where in C3 and C4 the users seemed to perform a greater portion of the totaldisplacement during the low-risk periods of the heart trajectory. There was high variance inperformance between the different users; the tasks may have been too difficult for the skilllevels of the participants, particularly the suturing task. Overall, the results show that thepulsing virtual fixture succeeds in relaying to the user the ideal times at which they shouldperform their tasks. The pulsing virtual fixture also improves completion time and accuracyof simpler tasks such as circle drawing.72Figure 7.4: Results for virtual fixture user studies. Left column shows circle taskresults, right column shows suture task results. Row I shows efficiency; row IIshows accuracy; row III shows damage; and row IV shows safe motion results(green: low-risk periods, red: high-risk periods).73Chapter 8Incorporating Motion Measurement8.1 IntroductionWhile the results from the initial user studies on the proof of concept system describedin Chapter 6 are very promising, the system must be developed further to progress from abench-top proof of concept project to a clinically feasible system. There are a number ofsimplifying assumptions in the initial system, as well as shortcomings which are identifiedSection 6.2. The next generation of the system, discussed in this chapter, incorporates anexternal device as the heart target, as well as an optical sensor to feed real time positiondata of the heart surface to the da Vinci controller.We believe this is the first attempt to use external 3D sensing to command robotic manip-ulators for both visual and heart motion stabilization for bimanual teleoperation. In previouswork, optical sensing was used to perform image processing to electronically stabilize thesurgeon’s view of the surgical site [12]. In the initial system described in Chapter 6, thecamera was mounted at a fixed distance from the heart target, and both the camera and theheart target were attached to a slave manipulator on the da Vinci. While this verified theconcept of mechanical visual stabilization, it assumed a perfect motion measurement en-vironment. The second generation of the heart motion compensate system eliminates thisassumption and studies the effect of motion measurement errors for visual stabilization.Further, since an external device is being used to simulate the heart, this relinquishes onePSM for other uses. For the study described herein, one PSM is used to carry the camera,74and the other two are used for bimanual teleoperation tasks. Initially the da Vinci StandardECM was going to be used to move the camera, however the mechanical design of theECM consists of ball screw actuation for the insertion joint. This results in the ECM beingboth too heavy and too slow to even hope to achieve the dynamics required to track theheart surface. Still, even with one PSM tasked as the camera arm, the ability to performbimanual teleoperation tasks provides opportunities to study the system in the context ofmore realistic clinical tasks, particularly suturing.8.2 Implementation8.2.1 System OverviewThe heart motion compensation system now consists of four computers, three responsi-ble for controlling two robots and the 3D position sensor, and one with a frame grabber. Aphotograph and a diagram of the system are shown in Figure 8.1, Figure 8.2 and Figure 8.3,Figure 8.1: Photograph 1 of generation two of the heart motion compensation System75Figure 8.2: Photograph 2 of generation two of the heart motion compensation systemFigure 8.3: Block diagram of generation two of the Heart Motion Compensation sys-tem76respectively. The beating heart is simulated using a Novint Falcon haptic device, controlledby PC 1. The taskboard is mounted on top of the Novint in Figure 8.2. LED markers areaffixed to the Novint Falcon, and are tracked by the (Northern Digital) NDI Certus Opto-trak tracker. The data from the Optotrak is read via serial bus in PC 2. There is a TCP/IPinterface between PC 2 and PC 3, over which PC 2 broadcasts the position of the Novint,as measured by the Optotrak. PC 3 streams in the Optotrak data, transforms it to be consis-tent with the dVRK coordinate system (using a coordinate transformation obtained throughearlier calibration), and uses these positions to command the Cartesian displacement of theslave manipulators of the dVRK. In case the arm is also being teleoperated, the Cartesiandisplacements are superimposed upon the teleoperation commands. The subsequent sub-sections will discuss each component of the system in more detail. The fourth PC with aframe grabber records the surgeon’s view of the task.8.2.2 The Heart: A Novint FalconPreviously, a da Vinci PSM was used to carry a simulated heart target which was mountedto the shaft of an instrument. Issues with roll of the instrument shaft, using up a PSM, andhaving the heart target and heart motion compensation system on the same robot were alldisadvantages of that idea, however it was useful in validating the proof of concept setup.To have a more clinically realistic setup, this generation of the heart motion compensa-tion system uses an external device, the Novint Falcon, seen in Figure 8.4. While moreanatomically realistic cardiac simulators are available, they are very expensive. A customrobot could have been designed and built to precisely recreate the 3D trajectory of the heart,however the Novint offered an affordable, simple solution to the heart simulation problem.The Novint Falcon is a low-cost, 3D haptic device that is used for consumer and hapticapplications. Offering 3D position and force feedback in a 4”× 4”×4” workspace throughFigure 8.4: The Novint Falcon77three parallel linkages, the Novint Falcon is often used for gaming and teleoperation ap-plications. The Falcon drives its arms with three capstan drives and optical encoders forposition sensing. The motor currents are commanded at a 1 kHz servo rate. Software de-veloped by Novint, specifically the low level driver software called HDAL (Haptic DeviceAbstraction Layer), handles the serial communications between the Falcon and the con-troller computer. A C++ API is provided for HDAL, and this has been used to encapsulatethe Falcon object in a Matlab Simulink s-function block, shown in Figure 8.5. Once inSimulink, a PID was tuned to control the Falcon to track the desired heart motion. Spectralline control, as in [52], was used for the Falcon, but this was contingent on not only errorsin the Falcon but also in the calibration procedure, discussed later in Section 8.2.8.Figure 8.5: Simulink control scheme for the Novint Falcon. An initialization phase isincluded, moving the Falcon to the desired start point with progressively moreaggressive control78Figure 8.6: NDI Optotrak Certus8.2.3 The Sensor: NDI Certus OptotrakThe NDI Optotrak Certus R©(seen in Figure 8.6) is a powerful research-grade motioncapture system that features exception spatial and temporal accuracy. With its unsurpassedaccuracy, it is used as the gold standard for motion tracking among research scientists [67].The Certus boasts an accuracy of up to 0.1 mm and a resolution of 0.01 mm, performingreal-time tracking of LED markers at frequencies up to 4600 Hz. NDI also provides an API,the Optotrak Application Programmer’s Interface (OAPI) which allows custom-written C++code to interface with the Optotrak sensor information.Using this API, I implemented software which initializes the Optotrak, forms a TCP/IPsocket connection with the PC running the dVRK (the server is created on the dVRK PC, theOptotrak PC connects as the client), and streams the Optotrak continuously to the dVRK.There is unidirectional traffic across this socket, with data being written by the Optotrak PCand read by the dVRK PC. The socket is designed such that the dVRK PC always readsin the most recent position data. To be specific, the data being streamed from the OptotrakPC is the centroid of the 3D positions of the four markers arranged symmetrically around apoint of interest, seen in Figure 8.7.79Figure 8.7: NDI Optotrak Certus marker configurations. Top: close up of dVRKmarker mount (O6 of the instrument can be seen centered between the mark-ers). Bottom: Both dVRK and Novint Falcon marker mounts8.2.4 The Robot: the dVRKThe dVRK system has been described in previous chapters, though for the first time inthis work all manipulators available (with the exception of the bulky ECM arm) are includedin the system. The user may perform bimanual teleoperation tasks, with both the left andright master manipulators. This controls two slave arms, while the third slave is used tohold and position the camera (the same one as in the initial user studies is used here, andmay be seen in Figure 5.12).On the software side, a dispatcher component was created whose sole purpose is readinginformation from the NDI data stream over the TCP/IP socket and broadcasting it to theother components, as requested. The dispatcher opens the server on the dVRK computer ata specified port, and the NDI computer connects to this as a client. Once the dVRK codesees that the connection has been established, it starts reading from the socket at a frequencyof 1 kHz. A timing analysis showed that new commanded positions were being supplied ata mean frequency of 333 Hz, which coincides with the loop frequency of the kinematics ofthe robot which is also 333 Hz. In cases where a new position was not received in time, theprevious position was used.The heart motion is not superimposed upon the slave arms until the user presses the“Start” button in the user interface, shown in Figure 8.8. This should not be initializeduntil the Novint Falcon target is stationary in its zeroed position, because the dispatcher80Figure 8.8: Windows of the GUI for the second generation heart motion compensationsystemcomponent collects the first 100 ms of data to compute the origin of the streamed Novintposition. The dispatcher then broadcasts to the relevant slave manipulators the displacementof the heart target, relative to that origin.8.2.5 CalibrationThe measured displacement of the Novint must be commanded to the dVRK in termsof the PSM coordinate frames. Thus, a calibration must be performed to find the trans-81Figure 8.9: Automated calibration trajectory for the dVRKformation between the NDI coordinate frame and the dVRK PSM coordinate frame. Asemi-automated calibration procedure was developed to find TNDItodV RK , or the 4×4 trans-form that transforms positions measured with the NDI to the dVRK coordinate frame.The first step in the procedure is to command the dVRK PSM to travel a sinusoidaltrajectory in the x, y, and z directions. The NDI marker plate from Figure 8.7 is mountedon the instrument shaft while the PSM performs this trajectory. The sinusoid has a peak topeak distance of 40 mm, and moves slowly enough the maximum/minimum points will becaptured by the NDI. Five periods of the sinusoid are performed in each dimension, with azero position commanded for the other two dimensions. A plot of the calibration trajectoryis seen in Figure 8.9. The PSM Cartesian positions as measured by the forward kinematicswith the encoders in the joints of the manipulator, as well as the measured centroid positionof the NDI markers are both recorded.Next, an automated algorithm processes the recorded data. The algorithm first partitionsthe logged data into three separate datasets, each one corresponding to the five sinusoidsin one dimension. Next, iterating through the datasets, the algorithm extracts the maxi-mum/minimum points in each period in the two measured trajectories (one measured bythe sensors in dVRK and one by the NDI), which are then normalized and correspond to±iˆ,± jˆ,±kˆ, or the three mutually perpendicular unit vectors of the basis of each of the twoCartesian coordinate systems.82At this stage the problem is a registration of two corresponding point sets. Also known asWahba’s problem, this optimization problem seeks to find the optimal rotation and transla-tion between corresponding 3D points [68]. Starting with point sets D, of the dVRK points,and N, of the NDI points. The problem becomes solving forD = RN + t (8.1)where R is the 3×3 rotation matrix, and t is the 3×1 translation vector for the rigid trans-formation between the two point sets. So the transform, T , is defined asT =R00 R01 R02 t0R10 R11 R12 t1R20 R21 R22 t20 0 0 1(8.2)Letting PD and PN being the 3D point sets for the dVRK and the NDI, respectively, eachwith n points, the centroid of PD, cD, and the centroid of PN , cN may be found bycD =1nn∑i=1PiD (8.3)cN =1nn∑i=1PiN (8.4)The covariance matrix, H, may then be obtained usingH =n∑i=1(PiD− cD)(PiN− cN)T (8.5)The optimal rotation, R is then found using Singular Value Decomposition (SVD),[U,S,V ] = SV D(H) (8.6)R =VUT (8.7)and the translation, t is found usingt =−RcD + cN . (8.8)83This 4×4 transform is saved as a .txt file, which is read in through the GUI before theheart motion compensation is enabled. Since the origin is calculated using the first 100ms ofdata, as described above, the translation component of this transform is not really used. In-stead, the 3×3 rotation matrix is critical in converting the NDI measurements to the dVRKspace. This rotation matrix is stored in the dispatcher component, and before positions arebroadcast to the slave arms, the measurement is transformed by the dispatcher to the dVRKspace.The NDI data collected during the calibration procedure is transformed using the com-puted TNDItodV RK and compared to the actual dVRK data. To find the angular error, thesix principles vectors, ±iˆ,± jˆ,±kˆ, from the dVRK data and the transformed NDI data arecompared in corresponding pairs, e.g. [+iˆdV RK ,+iˆNDI], [− jˆdV RK ,− jˆNDI]. The maximumdirection cosine error between each pair is computed. The maximum angular error over allthe six pairs is computed to be 0.62 degrees, which corresponds to 0.12 millimetres overthe 11 millimetre range of the heart motion in z.8.2.6 Dealing with DelayAs a mechanical system with mass and inertia, there is an inherent delay of actually mov-ing the manipulator to a desired position. With its cable drive mechanism the delay is small,however in the context of heart motion, where the heart moves over 10 mm about every sec-ond, a delay of tens of milliseconds can have a considerably detrimental effect. In the initialstudy, where the heart target was also being moved by a da Vinci manipulator, this inherentmechanical delay was consistent between the “heart” and the manipulator compensating forthe motion, so this was not a huge issue. Now, with an external robot simulating the heart,the mechanical delay of the PSM is a large problem. System identification tests concludedthat a delay of 55 milliseconds occurs between commanding the manipulator to a positionand it actually achieving the position. A portion of this may be attributed to the mechanicaldelay, though the intrinsic delay in the joint level PID controller is also included in this.To compensate for the delay, the assumed periodicity of the heart beat is leveraged. Thelength of the delay, tdelay in milliseconds is known, as is the period of the heart beat, Theart .Therefore, to align the manipulator’s achieved position with the measured position of theheart, the controller holds the desired reference signal r(t) for Theart − tdelay millisecondsbefore sending it to the plant. This is displayed pictorially in the block diagram in Fig-84Figure 8.10: Block diagram of compensating for the system delayure 8.10. In terms of software implementation, a First In First Out (FIFO) data structure isutilized: the queue. When a new position is received from the NDI, r(t), it is pushed to theback of the queue, or enqueued. At 1 kHz the controller checks to see if the entry at thefront of the queue has been held long enough, and if so, pops it from the front, or dequeuesit, and commands the position to the arms, r′(t). The resultant position signals are shownin Figure 8.11, and their errors in Figure 8.12. A significant decrease in error may be seenwith the inclusion of the delay compensation, particularly in the z direction.−0.75− [mm]−1−0.500.51Y [mm]0 1 2 3 4 5 6 7 8−4−2024Z [mm]Time [sec]  r(t)r’(t)y(t)Figure 8.11: Received, r(t), commanded, r′(t), and measured, yt , position signals forthe dVRK. The r′(t) signal is the held r(t) signal. y(t) is the measured posi-tion of the dVRK PSM. Also, r(t) corresponds to the measured position of thesimulated heart target85−0.5−0.2500.250.5X [mm]−1−0.500.51Y [mm]0 1 2 3 4 5 6 7 8−2−10123Z [mm]Time [sec]  r’(t) − y(t)r(t) − y(t)Figure 8.12: Tracking error between heart target and dVRK position with and withoutdelay compensation. The measured heart position is r(t), the dVRK position isy(t). r′(t) is the command signal with delay compensation.8.2.7 Spectral Line ControllerThe details of this controller have been explained previously in Section 5.4 and will notbe repeated here. The implementation of the controller differs for this generation of thesystem, however, because the desired position signal for the dVRK is obtained from thetransformed measured position of the Novint Falcon. Thus, the spectral line controller canbe used on the Novint Falcon to try to optimize the trajectory being measured by the NDIand transformed into dVRK space.Figure 8.13: Plant for system identification in spectral line controller development86Figure 8.14: Bode plots for the plant depicted in Figure 8.13In this case, the plant being inverted describes the Novint Falcon controlled by its PDcontroller, the measurement model of the NDI, as well as the coordinate transformationto the dVRK space, as seen in Figure 8.13. The same system identification process aswas used, and the resultant bode plots for the x, y, and z axes are shown in Figure 8.14.With the aim of trying to have the da Vinci PSMs follow a trajectory that is similar to arealistic heart trajectory, demonstrated in the initial user studies, the new approach is builtupon the initial system. Previously, a spectral line controller was used to compute a pre-conditioned input for the dVRK which resulted in sub-millimetre tracking error. Since thedVRK system has not changed, the goal is to have the added modules try to maintain thesame input to the dVRK as used previously. Thus, the pre-conditioned dVRK input wasdefined as the desired output of the Novint-NDI-Transform modules, and a spectral linecontroller was used to compute a pre-conditioned input for the Novint. Figure 8.15 showsthe outputs of the Novint-NDI-Transform system, or the input to the dVRK, r(t), when thepre-conditioned Novint input is used (blue), and when the original heart trajectory is usedas the Novint input (red). These are shown in comparison to the original heart trajectory(green). Recall that even in the initial studies, the actual heart trajectory is not used asinput to the dVRK. It is only shown in Figure 8.15 for reference. It can be seen that whenthe spectral line controller is used, higher frequency components which were interleaved inthe position signal, introduced by noise and errors in the Novint, the NDI sensor and thecoordinate transformation, are reduced. A discussion of final tracking error is discussed in87−0.6 −0.4 −0.2 0 0.2 0.4−101−6−4−2024 X [mm]Y [mm] Z [mm]Input to dVRK − Preconditioned Input to FalconInput to dVRK − No preconditioningActual Heart TrajectoryFigure 8.15: Comparing the input to the dVRK when spectral line control is used onthe Novint (blue), and when just the original heart trajectory is commanded tothe Novint (red), with the actual heart trajectory (green). Deviations from theactual heart trajectory only indicate how well the heart simulator emulates arealistic heart motion.the next section.8.2.8 System PerformanceWhen evaluating the performance of the system, the tracking error between the desiredand achieved signals is the metric to be considered. Specifically, the difference between theheart target (Novint) position and the dVRK PSM position. There are four major compo-nents to consider when discussing this tracking error:1. the Novint’s positioning accuracy (using its optical encoders and its proprietary kine-matic model)2. the accuracy of the NDI measurements of the markers rigidly attached to the Novint3. the accuracy of the coordinate transformation from the NDI Cartesian coordinatesystem to the dVRK Cartesian coordinate system4. the dVRK PSM positioning accuracyThese components are denoted in colour in the full system block diagram in Figure 8.3. TheNDI and coordinate transformation may be conceptually abstracted into one “motion mea-surement” component. While each component boasts sub-millimetre accuracy, as described88in their respective sections, the cascading effect of these errors makes it very challengingto achieve sub-millimetre accuracy between the heart target and the dVRK PSM position.Even with finely tuned PID gains, delay compensation and the spectral line controller, toimprove the overall tracking error to be sub-millimetre will require improvements to the in-dividual components. The system and framework presented here is nonetheless a promisingfoundation upon which to build. Further, due to the introduction of these errors incurredfrom a realistic, imperfect motion measurement system, a heart motion frequency of 0.75Hz was used. Slightly slower than for the initial studies, such a frequency could be clinicallyachieved using beta-blockers. The 1.5 Hz was an overestimate to begin with; discussionswith clinicians say 1.0 Hz would be ideal for tests. For preliminary tests of the closed loopmotion compensation system, a slightly slower frequency will be used.In addition to the actual heart trajectory, h(t) there are five different signals of interest:rN(t), yN(t), yN,NDI(t), yN,dV RK(t) = r(t), and y(t), all denoted in Figure 8.3. The followingfigures display the tracking error between various signals, and for the sake of clarity they aredepicted in separate figures. The first two figures show the tracking error of the first compo-nent: the Novint Falcon, between the preconditioned input commanded to the Novint and itsachieved position, as reported by its own position sensors. This is simply a representationhow well the heart simulator emulates the heart motion. The next two figures demonstratethe errors in the motion measurement component of the system (which includes NDI sensorand coordinate transformation). The final two display the tracking error of the dVRK PSM.Table 8.1 summarizes the maximum tracking errors in each dimension for each of thesethree modules.Table 8.1: Maximum Tracking Errors for Closed Loop Heart Motion CompensationSystemX [mm] Y [mm] Z [mm]Novint Falcon 0.2697 0.5545 2.4915Motion Measurement 0.3426 0.3947 1.1778dVRK PSM 0.3636 0.9688 0.694489−0.5−0.2500.250.5X [mm]−1−0.500.51Y [mm]0 1 2 3 4 5 6 7 8 9−5−2.502.55Z [mm]Time [sec]  yN(t)rN(t)Figure 8.16: Commanded versus achieved position signals for the Novint Falcon−0.25−0.12500.1250.25X [mm]−0.5−0.2500.250.5Y [mm]0 1 2 3 4 5 6 7 8 9−2−1012Z [mm]Time [sec]Figure 8.17: Tracking error between commanded and achieved position signals forthe Novint Falcon90−5−2.502.55 x 10−4X Position [m]−1−0.500.51 x 10−3Y Position [m]0 1 2 3 4 5 6 7 8 9 10−5−2.502.55x 10−3Z Position [m]Time [sec]  h(t)TNDI_to_dVRK*yN(t)yN,dVRKFigure 8.18: Differences in position signals in the motion measurement component.h(t) is the commanded heart trajectory, TNDItodV RK · yN(t) shows the Novintself-reported position signal transformed into the dVRK space, and yN,dV RKshows the NDI measurements of the Novint transformed into the dVRK space(equal to TNDItodV RK · yN,NDI(t)). This shows the difference between where theNovint thinks it is moving, and where the NDI sees it move−0.4− [mm]−0.5−0.2500.250.5Y [mm]0 1 2 3 4 5 6 7 8 9−1−0.500.51Z [mm]Time [sec]Figure 8.19: Error between NDI measured position and Novint self-reported position,both transformed into the dVRK space (i.e. TNDItodV RK · yN(t)− yN,dV RK)91−8−404x 10−4X Position [m]−2−101x 10−3Y Position [m]2 3 4 5 6 7−0.085−0.081−0.077−0.073Z Position [m]Time [sec]  y(t)r(t)Figure 8.20: Commanded versus achieved position signals for the dVRK PSM. Thecommanded signal, r(t), is from the NDI measurements (transformed intodVRK space) of the Novint being commanded with its pre-conditioned input−0.4− [mm]−0.500.51Y [mm]2 3 4 5 6 7−0.6−0.300.30.6Z [mm]Time [sec]Figure 8.21: Tracking error between commanded and achieved position signals forthe dVRK PSM92−0.5−0.2500.25X [mm]−1012Y [mm]0 1 2 3 4 5 6 7−6−3036Z [mm]Time [sec]  HeartTransformed NDIdVRK MeasFigure 8.22: External validation of dVRK PSM following a heart trajectory8.2.9 External ValidationHaving found the transform through the calibration procedure described in Section 8.2.5,external validation of the dVRK motion and the Novint Falcon can be performed. Eachrobot was commanded to follow a heart trajectory, and the NDI markers were mounted oneach to record its motion with the NDI. The measurements from the NDI optical sensor weretransformed into the Novint or dVRK space, as required, and compared with each robot’sself-reported position measurement as they were commanded to follow a heart trajectory.The validation for the Novint may be found in Figure 8.19, and in Figure 8.22 for thedVRK. In both, the commanded heart trajectory is shown in green, the transformed NDImeasurements are shown in blue, and the position measurements reported by each robot areshown in red. These plots confirm the that the dVRK and Novint are capable of tracking arealistic heart trajectory with reasonable error.938.3 User Studies8.3.1 SetupA photograph of the full system setup was shown in Figure 8.1 and Figure 8.2, and animage of an expert user performing a test is seen in Figure 8.23. For the user studies, theNovint was positioned on the da Vinci patient side cart in line of sight of the NDI sensor,with the NDI markers rigidly mounted. Calibration between the Novint, NDI and dVRK isperformed before every user study. The PCs for all these devices are positioned so they canall be operated during the user study. A fourth PC contains a frame grabber which recordsvideo data of the surgeon’s view during the study, for post-processing.The three dVRK PSMs are positioned over the Novint; the center arm (green/PSM2)holds the camera while the right and left slave arms (yellow/PSM1 and red/PSM3) arepositioned to enter the surgical site at about a 45 degree angle. The user sits at the masterconsole and operates the right and left MTMs as if in a normal clinical setting. A LargeNeedle Driver was used in the teleoperated arms. The camera used was the same as in theinitial studies and was held vertically over the surgical scene. The camera was repositionedby moving the setup joints of PSM2 during the test as requested by the user to get a betterview of their task space. Three expert robotic surgeons with many years of experience withrobotic suturing, all right-handed, participated in the study. Novice robot users were notinvited due to the difficulty of the robotic suturing task; it was necessary the participants hadprevious expertise in suturing to evaluate the benefit of the motion compensation system.8.3.2 Task Design8.3.2.1 Task TargetThe target application of this system is anastomosis for CABG surgery. The test taskwas therefore to perform bimanual suturing on a phantom under various conditions. Animage of the phantom is seen in Figure 8.24; it is essentially a piece of dense foam coatedin a coloured latex coating. 2-0 Vicryl sutures were used, and participants were asked tocomplete a double throw then single throw suture knot. An image of the procedure is shownin Figure 8.25. The user begins by wrapping the suture thread twice around the instrument94Figure 8.23: User performing a suturing tasktip, then reaches and grabs the loose end of the suture. After pulling tightly, another singleloop is made around the instrument and again the loose end is pulled through. Both sidesare pulled tight to secure the knot. The entry point for the suture was specified by a coloureddot on the phantom. Test ConditionsThe participants completed three practice sutures and five test sutures for each conditions.A completed test target is seen in Figure 8.26. The conditions are• C1: Everything stationary - gold standard; comparable to working on a stopped heart95• C2: Moving target, no compensation - emulates working on a beating heart withoutany aid from the robot• C3: Moving target, with compensation - the condition that will validate this system;looks at performing tasks on the beating heart with robotic compensation enabled. MetricsThree objective and two subjective measures were used to evaluate the participants’ per-formance. The objective measures (and their units of measure) are• Time of completion [sec]• Total displacement of the MTMs [m]• Accuracy of entry point [mm]• Number of errors.Figure 8.24: User study task phantom96Figure 8.25: Steps for tying the suture knot, images taken during user studies. Top-left: double throw around instrument. Top-right: Grab loose end. Bottom-left:Pull tightly. Bottom-right: make single loop and grab loose end againFigure 8.26: Completed task phantom97The accuracy of the exit point would have been a desirable metric as well, however theneedle size and tissue phantom properties were not very realistic, and would have skewedthe performance results for the exit location. The subjective metrics aim to evaluate the easewith which the user could complete the task under the various conditions. The subjectivemeasures are• Perceived ease• Perceived strain.These were self-reported by the participants using a Likert scale after all the conditionswere completed.8.4 Results and Discussions8.4.1 ResultsThe following figures depict the results for the objective metrics. For the time data, thetime for each trial was normalized relative to the mean of the five trials in condition one,the gold standard condition, for each user. The box plot in Figure 8.27 shows the resultsover all fifteen trials, five per user, for each condition. There was a significant amount ofvariance in the results, as can be seen in the whiskers of the plot. This variance stems fromskill level of the surgeon, and external factors which could not be normalized. For example,on occasion, the throw of the suture would slip off the instrument. This was unrelated tothe condition since it has no interaction with the tissue, the looping of the suture (steps a tod in Figure 8.25) is done in mid-air. This would cost the surgeon time. Figure 8.28 showsa scatter plot of the average normalized time for each condition, per user. The benefit ofthe motion compensation seems to depend on the skill level of the user, but this would bebetter understood with more participants. In Figure 8.28, user 2 was the least experiencedsurgeon.Figure 8.29 shows the sum of displacement of, or distance travelled by the MTMs duringthe suture tying. Again the data has been normalized relative the gold standard performanceWe were interested to see if the user’s ease with a task was related to how much they movedtheir hands while completing it. An indirect measure of efficiency, if the user was morehesitant or slower in completing a task this would be reflected here. It appears the same98random factors – e.g. suture thread slipping off the instrument – which affected the timingresults also affected the displacement results. The variance in the results is large, thoughthe figure shows the median total displacement (sum of left and right motion) decreasesmarginally when motion compensation is enabled. Human learning may also be a factoraffecting these results; there was a correlation between median displacement and how manysutures have been completed in the study.The accuracy results were initially going to be measured in millimetres, however it wasdiscovered that when the user punctured the foam at the indicated location with the needleand then tied the suture thread tight, the foam deformed and it was very difficult to get anaccurate measurement of where the needle actually punctured the target. From watchingthe recorded videos, the success of each target could be classified as either “hit”, “missed”or “other”. A target was considered hit if the needle perforation was completely within thespecified target, missed if completely outside, and other if it was touching the edge of thetarget. Recall each user was asked to tie five sutures per condition. The number of hit targetsis shown in Figure 8.30. While not as good as the gold standard results, the compensationis shown to have an advantage over the non-compensated condition.C1 C2 C30.60.811. Normalized Time of CompletionNormalized TimeFigure 8.27: Suturing results: Time of completion box plot. The red line representsthe median of the data, the blue edges of the box show the 25th and 75th per-centiles, the whiskers extend to the most extreme data points not consideredoutliers (which are depicted with a red cross)99C1 C2 C30.911. Scatter Plot of Normalized Time of CompletionNormalized Time  User 1User 2User 3Figure 8.28: Suturing results: Time of completion scatter plot.The number of errors counts the number of additional perforations of the heart surfaceas well as missed grabs of the suture/needle due to it moving away on the heart surface.The results in Figure 8.31 show the mean number of errors per trial for each condition. AnC1 C2 C30.40.60.811. DisplacementC1 C2 C30. C2 C30.60.811.21.41.6Total MTMFigure 8.29: Suturing results: MTM displacement. MTML: Left Master Tool Manip-ulator, MTMR: Right Master Tool Manipulator100observable decrease in the number of errors performed by the users is experienced whenmotion compensation is enabled, uniformly across all the users. This suggests that themotion compensation system decreases the risk of damaging the heart surface.Finally, the last figure shows the results from the Likert scale, measuring self-reportedperceived ease and strain for each user. The Likert scale included five options, and eachoption was assigned an integer value: strongly agree = 1, agree = 2, neutral = 3, disagree= 4, strongly disagree = 5. Thus, a lower score indicates more comfort, more ease, andgenerally a higher subjective preference for a condition. The results in Figure 8.32 onceagain show the difference in skill level and preference between the users. The averageresults show a tendency towards a higher preference for the motion compensated condition,those still not as much as for the gold standard condition. In conversation with the users aftertheir tests, they mentioned that the residual motion of the heart in the motion compensatedcondition (due to the the millimetre-level tracking errors) was almost as distracting as thefull heart motion. They could recognize the amplitude of the motion was significantly lesswhen compensation was enabled, but the fact that it was still moving, even slightly, made itmore difficult than the fully stationary condition.1 2 3012345 Number of Hit Targets# Hit TargetsUser  C1C2C3Figure 8.30: Suturing results: Accuracy101C1 C2 C300.511.522.53 Average Number of Errors Per TrialAvg Errors Per Trial  User 1User 2User 3Figure 8.31: Suturing results: Number of errorsC1 C2 C311.522.533.544.55 Perceived EaseEaseC1 C2 C311.522.533.54 Perceived StrainStrain  User 1User 2User 3MeanFigure 8.32: Suturing results: Likert scale. Left: task was easy. Right: task inducedstrain. Strongly agree = 1, agree = 2, neutral = 3, disagree = 4, strongly disagree= 5. A lower score thus indicates more comfort1028.4.2 DiscussionThe most obvious consideration is the sample size used in this analysis. Ideally upwardsof ten participants could be included, and they would perform twenty sutures in each con-dition. This would allow the effects of uncontrollable factors such as suture thread slippingoff instruments to be mitigated. Further, having more participants would allow a better in-vestigation of the relationship between years of experience and performance with the heartmotion compensation system. Unfortunately, the number of users in the Vancouver areawho have the necessary qualifications to test this system are few in number. For this studyall the known robotic experts in the area were invited, as were their residents who had suf-ficient robotic training. These individuals have very limited availability, so if they do havean opportunity to come test the system, the experiment cannot occupy hours and hours oftheir time. Giving an introduction to the experiment, performing the fifteen test sutures plusnine total practice sutures, and then completing the Likert questionnaire took approximatelyone hour of each participant’s time. Regardless, the results from this study have been veryuseful for learning critical lessons about studying surgeon’s ability to perform sutures.There are many factors contributing to performing a clean, successful suture. The tissuetype, suture thread, needle size, and type of knot are all highly important. Initially test tar-gets were made of medium-density foam covered in a thick (about 2 mm) latex coating. Thecoating was found to be so stiff the experts were having tremendous difficulty puncturingit with the needle. The alternative targets, made of dense foam with a light latex coating,were thus employed for these tests. While the users were able to actually guide the needlethrough the tissue, all the participants noted that the tissue stiffness was significantly higherthan anything they would deal with clinically, working in soft tissue. The suture threadused is also important; initially Monocryl thread was used, however this was so slipperythe experts could not make the knot remain tightly tied. Instead, Vicryl thread was usedin these studies. The needle size made less of an impact on the performance of the suture,though the size can help improve the clinical likeness of the test. Here, 2-0 sutures withSH 26 mm 1/2c taper needles were used, whereas for the anastomosis required in CABGsurgery needles half the size would typically be used. Finally, the type of knot performedwill directly affect the overall quality of the suture. The type of thread and the properties ofthe tissue must be considered when deciding which knot to use. For this study participantssuggested that a cinch knot would have resulted in tighter sutures on the phantom.103The participants’ perception of the tracking error for the motion compensated conditionswas also highly interesting. The residual motion from the tracking error they found to bevery distracting. More careful investigation should be performed to evaluate whether theerrors in the motion stabilization in the teleoperated arms, or in the visual stabilization inthe camera arm are more distracting to the user. Ideally both would be eliminated entirely,however practically the da Vinci and the measurement model have limitations. There areopportunities for improved tracking performance on the manipulators discussed in the nextsection to improve the motion stabilization. Considering electronic stabilization for theimprovement of the visual stabilization would also be beneficial.8.5 Conclusions and Future WorkThe results for the second generation heart motion compensation system are promisingfor the first clinically feasible system with real time motion measurement. The bimanualsuturing, a highly surgically relevant task, served as a good indicator of practical use ofthe system since it is a very realistic task which demands high accuracy and comfort withthe system since it is typically performed many times during a surgical procedure. Theresults show an increase in accuracy, a reduction in the number of errors performed, and asubjective preference for having motion compensation enabled when working on a movingtarget. As the first attempt at a closed loop motion measurement system, the results arepromising, though there are many improvements which can be made to move closer toclinical feasibility.One of the first things to investigate, low-hanging fruit, so to speak, would be to study theperformance of the system with the originally oriented heart trajectory. Recall that the hearttrajectory was rotated such that the maximal variance was in the z-direction, correspondingto the insertion direction on the PSMs. This resulted in about a range of [0.8, 2.4, 10.8]mmin the x, y, and z directions, respectively. In the original trajectory, the range is [2.3, 4.7,9.2]mm in the x, y, and z directions, respectively. The range in x for the rotated trajectory issmaller than the positioning accuracy that can be achieved by the overall system. Further,once the teleoperated slaves are put into the approximately 45 degree entry position for theirtask, the z-direction no longer corresponds to the insertion joint, rather more to the outeryaw, which carries more weight and has to overcome more inertial effects. Investigatingthe potential improvement in tracking results when the original trajectory would be highly104interesting, and would not require any software, electrical, or mechanical changes to thesystem. Simply the input to the Novint would have to be modified.On the theoretical side, it would be highly advantageous to develop a closed loop con-troller for the da Vinci, one more advanced that the basic joint-level PID already in place.A controller which could adapt with information from previous trajectory, learning fromits tracking errors from the nearly periodic command signal, could potentially improve thetracking performance. The concept of harmonic oscillators, a controller with a pole placeddirectly on the harmonic frequencies of the heart trajectory, theoretically achieving infinitegain at these frequencies, was also considered to further reduce tracking error, but has notyet been implemented due to time constraints. A state space formulation of such a controllercould be made adaptive.Some participants were complaining about the residual motion in the visual stabilizationcaused by the small tracking errors in the system. Looking into electronic stabilization, i.e.performing image processing to ensure the same spot on the target is in the same position inthe viewing screen every frame would complete the illusion of visual stabilization. Issuesof image processing latency must be considered carefully, though.System level changes may also be investigated. While the Novint and the NDI wereselected due to their availability, convenience and believed good performance, the systemanalysis shows their sub-millimetre accuracies may not be sufficient for this application.Considerations should be made to look into a more accurate sensing technology, one whichcould ideally achieve accuracy in the order of tens of microns. With the goal of eventuallyworking on porcine models, perhaps a more tissue-like phantom and heart simulator couldbe developed which would smooth the transition between bench top and clinical testing ofthe system. Finally, allowing the surgeon the ability to move the camera themselves wouldbe advantageous to improve the clinical feasibility of the system. During the user studiesthe experienced surgeons began pressing the camera-controlling foot pedals from habit; op-erating the camera zoom and positioning its view to the ideal position is almost as importantto the ease and comfort of completing a task as teleoperating the actual surgical instruments.While the prototype camera used in these studies does not offer many configurable settings(there is no zoom available, and the auto-corrections on the image cannot be modified), nowit is mounted on a PSM the participant could be allowed to position the PSM as desired.This may be a complex addition to the system to properly incorporate the camera view into105the kinematics of the teleoperation.106Chapter 9Conclusion and Recommendations9.1 ConclusionsThis thesis work discusses the development of a heart motion compensation system im-plemented on a da Vinci R©Research Kit controlling a clinical da Vinci Classic System, forthe application of anastomosis during minimally invasive CABG surgery on a beating heart.The preliminary research hypothesis of whether the robot was capable of tracking theheart motion is proven to be true by achieving tracking error on the order of one millimetreof a realistic 3D heart trajectory. This heart trajectory corresponded to a published 3D hearttrajectory, measured on a point on the left ventricular midwall. The trajectory is assumed tobe periodic with a frequency of 1.5 Hz with a maximum displacement of 11 mm. A spectralanalysis of this heart trajectory was performed, and a spectral line controller was developedwith the findings. System identification on the da Vinci slave arms was performed to obtaina model of the system, and using the principle of inversion at the heart spectral frequencies,the open-loop spectral line controller computed a pre-conditioned input which minimizedthe tracking errors.With the slave arms capable of following the heart trajectory, the surgeon’s teleoperationcommands were superimposed upon the automated trajectory. A stereo endoscopic camerawas also commanded to follow the heart trajectory, emulating working on a stopped heart.107The secondary research question was whether the motion compensation system wouldbe positively received by robotic surgeons, and how a semi-automated, shared control tele-operation scheme would compare to the gold standard of performing surgery on a stoppedheart. Initial user studies with expert robotic surgeons showed very promising results, withsignificant improvement in accuracy from the uncompensated to compensated conditions.The accuracy and completion times were not as good as the gold standard results of workingon a stopped target, however the surgeons were excited about the system and its potentialimplications for the field of robotic cardiac procedures.The second generation of the system, incorporating real time motion measurement, presentsa more clinically feasibly closed loop system. An external target (a commercially availablerobot) is used to simulate the beating heart, a 3D optical position sensor (NDI Certus Op-totrak) measures the displacement of the heart, the measurements are transformed into thedVRK coordinate space (using a coordinate transformation determined during a calibrationprocedure), and then the dVRK slave manipulators are commanded to compensate for theheart motion. Delay compensation was integrated to handle the temporal delays totalling55 milliseconds that are present in the system. The periodicity of the heart motion signalis used to accomplish this; information from the previous period is used to command therobot to minimize the tracking error. All three PSMs are used in the second generation sys-tem: one arm carries the small endoscopic camera from the initial studies (the assumptionof perfect motion measurement is removed), and the other two arms are teleoperated by thesurgeon. This setup allowed for bimanual suturing to be performed on a foam target duringthe user studies. A highly pertinent clinical task, the results showed the motion compen-sation improved the participants’ accuracy and reduced the number of errors performed.Subjectively, the users found themselves to be more strained when motion compensationwas enabled, compared to the gold standard of a stationary target, due to the tracking errorsand motion measurement errors.As an additional safety feature, virtual fixtures were considered to attempt to reduceaccidental perforations of the heart surface. With the virtual stabilization provided by theheart motion compensation system, as well as with the errors present in the system, it wassometimes difficult to perceive when the heart surface is accelerating upwards towards theinstrument tool tip. As an aid for the surgeon, the virtual fixtures would provide hapticfeedback during periods of acceleration upwards towards the instrument, indicating a “highrisk” portion of the trajectory. This was a dynamic, non-continuous virtual fixture which108was also verified in user studies. Results showed the fixtures were helpful in relaying whenthe operator should be performing the majority of their hand motions.Another contribution was the development of a Simulink to C++ interface for rapid pro-totyping of controller design. The controller design and development process in the contextof the dVRK cisst/SAW framework was very tedious and time-consuming. This interface,based on TCP/IP communication, effectively outsources the control logic to Simulink, afriendly block-diagram environment which also offers many useful toolboxes for controlleranalysis and design. Experiments showed a 1 kHz control frequency could be maintainedwhen the Simulink controller was commanding the robot. This will be a useful tool forcontrol system designers on the dVRK.An advantage of the work presented in this thesis is the employment of the da Vinci as therobotic platform for the heart motion compensation system. Already clinically prevalent,the da Vinci is poised as the perfect platform upon which novel automation schemes maybe introduced into clinical use. With thousands of da Vinci systems already in use, thepotential impact of a da Vinci based system may be highly significant. However, the workdone here was completed on the first generation da Vinci system, and must be modified towork on the newest, substantially different system.As a proof of concept development, this system still contains a number of limitations. Themodel of the manipulators used for control development were based on parametric modelsobtained using data-driven system identification. More accurate models may be obtainedwith nonparametric methods. The heart trajectory used for the developments was assumedto be periodic, when in reality the heart has a quasiperiodic motion signal. A closed loopcontroller will be necessary for clinical implementation, and vision based sensing may notbe ideal since occlusions will be a significant concern in clinical application. Nonetheless,as a base system the results were very promising. Future improvements that can be made toaddress these concerns are addressed in the next section.9.2 ContributionsTo summarize, the contributions of this thesis work are as follows• the development of an open loop spectral line controller which is proven to be ca-109pable of commanding the da Vinci patient side manipulators to track a realistic 3Dheart trajectory with submilletre tracking error, assuming periodic quasiperiodic tra-jectories.• a novel idea for visual stabilization, consisting of mechanically moving the camera tocompensate for the heart motion, keeping a fixed distance between the heart surfaceand the camera.• the development of additional safety measures to protect the heart surface, in the formof non-continuous moving wall virtual fixtures.• the creation of a controller development tool linking Simulink to the C++ dVRKcode, allowing the outsourcing of control logic to Simulink.• incorporating a real time, 3D optical position sensing modality to command the daVinci patient side manipulators to track the heart surface. Still using an open loopspectral line controller, the closed motion measurement system is now more clinicallyfeasible. Tracking errors on the order of one millimetre were achieved for this system.• developed a reasonable approach to human studies, evaluating the overall integratedsystem and controller with expert robotic surgeons to evaluate the system’s abilityto help the surgeon work on a moving target. Clinically relevant, complex biman-ual teleoperation tasks were completed by the surgeons in the form of suturing foamtargets. The results from the studies show significant improvement between com-pensated and uncompensated conditions when working on the moving target. Thesystem can be further improved by iterating the control scheme using the Simulinktool• demonstrated the feasibility of a heart motion compensation system on the da Vincisystem, an FDA clinical system that has been used in thousands of surgeries.9.3 RecommendationsA number of improvements and further advancements are necessary before the systemcan be considered for clinical use. The long term developments would hopefully lead toporcine model testing, followed by clinical studies, obtaining FDA approval and ultimatelyclinical practice. The most prominent is the incorporation of motion measurement of the110heart. A clinically feasible solution for acquiring the position signal corresponding to theheart surface trajectory will be challenging. Past research shows tissue tracking using stereoendoscopic cameras is possible, particularly when augmented with information from elec-trocardiogram signals. Further controller development to minimize latency and attempt toimprove tracking errors with a closed loop controller would also be required.More immediately, user studies with more surgically relevant tasks, such as an actualanastomosis including more complex articulated instrument motions and instrument/tissueinteractions, would be a better indicator of the quality of the system. One of the expertsurgeons who participated in the second generation user studies suggested either real hearttissue and a pump flowing blood through the vessels, or else obtaining some simulatedvessels available from Ethicon, used in regular surgical training. Further, he recommendedperforming experiments with the original, non-rotated heart trajectory signal. The truedifficulty of the task on a beating heart, without robotic compensation, is not well portrayedwith so little motion in the x- and y- directions.While there are many challenges still to face, many of these may be mitigated by sim-plifying the core motion compensation problem. Novel, minimally invasive mechanicalstabilizers can be designed which would constrain the heart motion, minimizing the am-plitude of the motion compensation required. Further, pacemakers and drugs such as betablockers can be used to slow down and pace the heart. The lower frequency, lower ampli-tude motion will result in simplified trajectories for the robot to track, and hopefully evenbetter tracking results.The targeted application is minimally invasive CABG surgery on a beating heart, sincethis is the most common revascularization procedure, although this can also be utilized forother cardiac applications such as mitral valve repair. The motion compensation principlesdeveloped may also be employed for respiration compensation. It is acknowledged that notall patients will be eligible for minimally invasive surgery on the beating heart. Prior heartconditions, difficult access to the surgical site, and medical history might require a tradi-tional open procedure. However, if clinically accepted for robotic cardiac procedures, thissystem may have a largely significant impact, particularly in popularizing robotic surgeryin the cardiac space. Using the already prevalent da Vinci surgical system as the roboticplatform significantly improves the likelihood of such a system being adopted into prac-tice, since only the automation component on top of the system must be FDA approved. if111succcessful, the need for both the sternotomy and the CPB machine would be eliminated,and patients could potentially walk away from cardiac bypass surgeries the same day. Thisstands to not only greatly improve patient quality of life, but also significantly reduce hos-pital time and costs.112Bibliography[1] P. Kazanzides, H. Choset, T. Haidegger, and S. DiMaio, “Icra 2015 workshop:Shared frameworks for medical robotics research,” 2015. → pages iv, 42[2] A. Alwan et al., Global status report on noncommunicable diseases 2010. WorldHealth Organization, 2011. → pages 1[3] A. S. Go, D. Mozaffarian, V. L. Roger, E. J. Benjamin, J. D. Berry, W. B. Borden,D. M. Bravata, S. Dai, E. S. Ford, C. S. Fox, et al., “Heart disease and strokestatistics–2013 update: a report from the american heart association.,” Circulation,vol. 127, no. 1, p. e6, 2013. → pages 1[4] J. W. Kirklin and B. G. Barratt-Boyes, Cardiac surgery: morphology, diagnosticcriteria, natural history, techniques, results, and indications, vol. 1. WB SaundersCompany, 1993. → pages 1, 8[5] P. Cattin, H. Dave, J. Gru¨nenfelder, G. Szekely, M. Turina, and G. Zu¨nd, “Trajectoryof coronary motion and its significance in robotic motion cancellation,” EuropeanJournal of Cardio-thoracic Surgery, vol. 25, no. 5, pp. 786–790, 2004. → pages 3, 12[6] M. Lemma, A. Mangini, A. Redaelli, and F. Acocella, “Do cardiac stabilizers reallystabilize? experimental quantitative analysis of mechanical stabilization,” InteractiveCardioVascular and Thoracic Surgery, vol. 4, no. 3, pp. 222–226, 2005. → pages 3,12[7] C. Gao, M. Yang, Y. Wu, G. Wang, C. Xiao, Y. Zhao, and J. Wang, “Early andmidterm results of totally endoscopic coronary artery bypass grafting on the beatingheart,” The Journal of thoracic and cardiovascular surgery, vol. 142, no. 4,pp. 843–849, 2011. → pages 3, 9[8] J. Gagne, W. Bachta, P. Renaud, O. Piccin, E´. Laroche, and J. Gangloff, “Beatingheart surgery: Comparison of two active compensation solutions for minimallyinvasive coronary artery bypass grafting,” in Computational Surgery and DualTraining, pp. 203–210, Springer, 2014. → pages 3, 13, 14113[9] C. N. Riviere, N. A. Patronik, and M. A. Zenati, “Prototype epicardial crawlingdevice for intrapericardial intervention on the beating heart,” in Heart Surg Forum,vol. 7, pp. E639–E643, 2004. → pages 3, 12[10] A. Trejos, S. Salcudean, F. Sassani, and S. Lichtenstein, “On the feasibility of amoving support for surgery on the beating heart,” in Medical Image Computing andComputer-Assisted Intervention–MICCAI99, pp. 1088–1097, Springer, 1999. →pages 3, 12, 59[11] S. G. Yuen, D. T. Kettler, P. M. Novotny, R. D. Plowes, and R. D. Howe, “Roboticmotion compensation for beating heart intracardiac surgery,” The Internationaljournal of robotics research, vol. 28, no. 10, pp. 1355–1372, 2009. → pages 3, 13,21, 51[12] Y. Nakamura, K. Kishi, and H. Kawakami, “Heartbeat synchronization for roboticcardiac surgery,” in Robotics and Automation, 2001. Proceedings 2001 ICRA. IEEEInternational Conference on, vol. 2, pp. 2014–2019, IEEE, 2001. → pages 3, 13, 21,74[13] O¨. Bebek and M. C. C¸avusoglu, “Intelligent control algorithms for robotic-assistedbeating heart surgery,” Robotics, IEEE Transactions on, vol. 23, no. 3, pp. 468–480,2007. → pages 3, 15, 51[14] N. I. of Health, “What is coronary artery bypass grafting?,” 2012. → pages 6[15] Heart and S. Foundation, “Percutaneous coronary intervention (pci or angioplastywith stent),” 2012. → pages6[16] N. C. for Health Statistics (US), “Number, rate, and standard error of all-listedsurgical and nonsurgical procedures for discharges from short-stay hospitals, byselected procedure,” 2009. numberrate.pdf. → pages7[17] M. E. Farkouh, M. Domanski, L. A. Sleeper, F. S. Siami, G. Dangas, M. Mack,M. Yang, D. J. Cohen, Y. Rosenberg, S. D. Solomon, et al., “Strategies formultivessel revascularization in patients with diabetes,” New England journal ofmedicine, vol. 367, no. 25, pp. 2375–2384, 2012. → pages 7[18] A. T. Ong, P. W. Serruys, F. W. Mohr, M.-C. Morice, A. P. Kappetein, D. R. Holmes,M. J. Mack, M. van den Brand, M.-A. Morel, G.-A. van Es, et al., “The synergybetween percutaneous coronary intervention with taxus and cardiac surgery (syntax)114study: design, rationale, and run-in phase,” American heart journal, vol. 151, no. 6,pp. 1194–1204, 2006. → pages 7[19] K. Lall, “Coronary artery bypass grafting,” 2013. → pages 7[20] MSurgery, “Vascular anastomosis,” 2015.→ pages 8[21] M. Net, “Coronary artery bypass grafting: How is cabg surgery done?,” 2015. artery bypass graft/page4.htm. → pages 7[22] D. Machin and C. Allsager, “Principles of cardiopulmonary bypass,” ContinuingEducation in Anaesthesia, Critical Care & Pain, vol. 6, no. 5, pp. 176–181, 2006. →pages 8[23] HealthInformatics, “Robot assisted surgery,” 2015. → pages 9[24] O. Ben-Porat, M. Shoham, and J. Meyer, “Control design and task performance inendoscopic teleoperation,” Presence: Teleoperators and Virtual Environments, vol. 9,no. 3, pp. 256–267, 2000. → pages 10[25] M. J. POTEL, J. M. RUBIN, S. A. MacKAY, A. M. AISEN, J. AL-SADIR, and R. E.SAYRE, “Methods for evaluating cardiac wall motion in three dimensions usingbifurcation points of the coronary arterial tree.,” Investigative Radiology, vol. 18,no. 1, pp. 47–57, 1983. → pages 10[26] D. Stoyanov, G. P. Mylonas, F. Deligianni, A. Darzi, and G. Z. Yang, “Soft-tissuemotion tracking and structure estimation for robotic assisted mis procedures,” inMedical Image Computing and Computer-Assisted Intervention–MICCAI 2005,pp. 139–146, Springer, 2005. → pages 10, 11[27] R. Richa, P. Poignet, and C. Liu, “Three-dimensional motion tracking for beatingheart surgery using a thin-plate spline deformable model,” The International Journalof Robotics Research, vol. 29, no. 2-3, pp. 218–230, 2010. → pages 11[28] G. Kurz, M. Baum, and U. D. Hanebeck, “Real-time kernel-based multiple targettracking for robotic beating heart surgery,” in Sensor Array and Multichannel SignalProcessing Workshop (SAM), 2014 IEEE 8th, pp. 201–204, IEEE, 2014. → pages 11[29] C. Borst, E. W. Jansen, C. A. Tulleken, P. F. Grundeman, H. J. M. Beck, J. W. vanDongen, K. C. Hodde, and J. J. Brede´e, “Coronary artery bypass grafting withoutcardiopulmonary bypass and without interruption of native coronary flow using anovel anastomosis site restraining device (octopus),” Journal of the AmericanCollege of Cardiology, vol. 27, no. 6, pp. 1356–1364, 1996. → pages 12115[30] T. Gilhuly, Optical and physical heart stabilization for cardiac surgery. PhD thesis,University of British Columbia, 1998. → pages 12[31] U. Hagn, R. Konietschke, A. Tobergte, M. Nickl, S. Jo¨rg, B. Ku¨bler, G. Passig,M. Gro¨ger, F. Fro¨hlich, U. Seibold, et al., “Dlr mirosurge: a versatile system forresearch in endoscopic telesurgery,” International journal of computer assistedradiology and surgery, vol. 5, no. 2, pp. 183–193, 2010. → pages 15[32] E. E. Tuna, T. J. Franke, O. Bebek, A. Shiose, K. Fukamachi, and M. C. Cavusoglu,“Heart motion prediction based on adaptive estimation algorithms forrobotic-assisted beating heart surgery,” Robotics, IEEE Transactions on, vol. 29,no. 1, pp. 261–276, 2013. → pages 16[33] F. Liang, Y. Yu, H. Wang, and X. Meng, “Heart motion prediction in robotic-assistedbeating heart surgery: A nonlinear fast adaptive approach,” International Journal ofAdvanced Robotic Systems, vol. 10, no. 82, 2013. → pages 16, 23[34] S. B. Kesner and R. D. Howe, “Robotic catheter cardiac ablation combiningultrasound guidance and force control,” The International Journal of RoboticsResearch, vol. 33, no. 4, pp. 631–644, 2014. → pages 16[35] M. Dominici and R. Cortesa˜o, “Cascade force control for autonomous beating heartmotion compensation,” Control Engineering Practice, vol. 37, pp. 80–88, 2015. →pages 17[36] S. Park, R. D. Howe, and D. F. Torchiana, “Virtual fixtures for robotic cardiacsurgery,” in Medical Image Computing and Computer-Assisted Intervention–MICCAI2001, pp. 1419–1420, Springer, 2001. → pages 17, 71[37] J. Ren, R. V. Patel, K. McIsaac, G. Guiraudon, T. M. Peters, et al., “Dynamic 3-dvirtual fixtures for minimally invasive beating heart procedures,” Medical Imaging,IEEE Transactions on, vol. 27, no. 8, pp. 1061–1070, 2008. → pages 18[38] T. L. Gibo, L. N. Verner, D. D. Yuh, and A. M. Okamura, “Design considerations andhuman-machine performance of moving virtual fixtures,” in Robotics andAutomation, 2009. ICRA’09. IEEE International Conference on, pp. 671–676, IEEE,2009. → pages 18[39] N. V. Navkar, Z. Deng, D. J. Shah, K. E. Bekris, and N. V. Tsekos, “Visual andforce-feedback guidance for robot-assisted interventions in the beating heart withreal-time mri,” in Robotics and Automation (ICRA), 2012 IEEE InternationalConference on, pp. 689–694, IEEE, 2012. → pages 18[40] F. Ryde´n and H. J. Chizeck, “Forbidden-region virtual fixtures from streaming pointclouds: Remotely touching and protecting a beating heart,” in Intelligent Robots andSystems (IROS), 2012 IEEE/RSJ International Conference on, pp. 3308–3313, IEEE,2012. → pages 18116[41] X. G. Xu and K. F. Eckerman, Handbook of anatomical models for radiationdosimetry. CRC Press, 2009. → pages 20[42] N. Ingels, G. T. Daughters, E. B. Stinson, E. L. Alderman, and D. C. Miller,“Three-dimensional left ventricular midwall dynamics in the transplanted humanheart.,” Circulation, vol. 81, no. 6, pp. 1837–1848, 1990. → pages 20, 24[43] Kidport, “The heart,” 2015. →pages 21[44] L. Cuvillon, J. Gangloff, M. de Mathelin, and A. Forgione, “Toward robotizedbeating heart tecabg: Assessment of the heart dynamics using high-speed vision,” inMedical Image Computing and Computer-Assisted Intervention–MICCAI 2005,pp. 551–558, Springer, 2005. → pages 23[45] P. Kazanzides, Z. Chen, A. Deguet, G. Fischer, R. Taylor, and S. DiMaio, “Anopen-source research kit for the da vinci r surgical robot,” in Proc. IEEEInternational Conf. on Robotics and Automation (ICRA14), 2014. → pages 27[46] Z. Chen, A. Deguet, R. Taylor, S. DiMaio, G. Fischer, and P. Kazanzides, “Anopen-source hardware and software platform for telesurgical robotics research,” inProceedings of the MICCAI Workshop on Systems and Architecture for ComputerAssisted Interventions, Nagoya, Japan, pp. 22–26, 2013. → pages 27, 42, 44[47] O. Mohareri, C. Schneider, and S. Salcudean, “Bimanual telerobotic surgery withasymmetric force feedback: A davinci R© surgical system implementation,” inIntelligent Robots and Systems (IROS 2014), 2014 IEEE/RSJ InternationalConference on, pp. 4272–4277, IEEE, 2014. → pages 31, 47[48] I. S. Inc., “User guide - project: The da vinci research kit, rev: B [confidential],” 2014. Guide DVRK Dec 2014.pdf.→ pages 32, 33, 34[49] R. M. Murray, “Chapter 6: Transfer function (course notes from cds101),” 2004. murray/courses/cds101/fa04/caltech/am04 ch6-3nov04.pdf. → pages37[50] A. K. Tangirala, “Principles of system identification,” 2015. → pages 37[51] L. Sciavicco and B. Siciliano, “Modelling and control of robot manipulators,” 1996.→ pages 37[52] A. Ruszkowski, O. Mohareri, S. Lichtenstein, R. Cook, and S. Salcudean, “On thefeasibility of heart motion compensation on the davinci R© surgical robot for coronary117artery bypass surgery: Implementation and user studies,” in Robotics and Automation(ICRA), 2015 IEEE International Conference on, pp. 4432–4439, IEEE, 2015. →pages 38, 50, 57, 68, 70, 78[53] J. Ziegler and N. Nichols, “Optimum settings for automatic controllers,” Journal ofdynamic systems, measurement, and control, vol. 115, no. 2B, pp. 220–222, 1993. →pages 39, 41[54] S. Bennett, A history of control engineering, 1930-1955. No. 47, IET, 1993. → pages41[55] A. Ruszkowski, Z. F. Quek, A. M. Okamura, and S. Salcudean, “Simulink to c++interface for controller development on the da vinci R©research kit (dvrk),” inRobotics and Automation (ICRA), 2015 IEEE International Conference on;Workshop on Shared Frameworks for Medical Robotics Research, IEEE, 2015. →pages 42[56] Z. Chen and P. Kazanzides, “Multi-kilohertz control of multiple robots via ieee-1394(firewire),” in Technologies for Practical Robot Applications (TePRA), 2014 IEEEInternational Conference on, pp. 1–6, IEEE, 2014. → pages 43[57] L. Qian, Z. Chen, and P. Kazanzides, “An ethernet to firewire bridge for real-timecontrol of the da vinci research kit (dvrk),” in Emerging Technology and FactoryAutomation, 2015. ETFA’15. IEEE International Conference on, IEEE, 2015. →pages 43[58] A. Deguet, R. Kumar, R. Taylor, and P. Kazanzides, “The cisst libraries for computerassisted intervention systems,” in MICCAI Workshop, 2008. → pages 43[59] I. S. Inc., “Welcome to the da vinci research kit wiki community,” 2015. Page. → pages 43[60] Matlab, “Simulink: Simulation and model-based design,” 2015. → pages 43[61] J. Rosen, B. Hannaford, and R. M. Satava, Surgical Robotics: Systems Applicationsand Visions. Chapter 21. Springer Science & Business Media, 2011. → pages 51, 53[62] A. Ruszkowski, “A. ruszkowski entry: Hamlyn 2015 - surgical robot challenge 2015- final,” 2015. → pages 57[63] E. M. Ritter and D. J. Scott, “Design of a proficiency-based skills training curriculumfor the fundamentals of laparoscopic surgery,” Surgical innovation, vol. 14, no. 2,pp. 107–112, 2007. → pages 60118[64] A. Ruszkowski, Z. F. Quek, A. M. Okamura, and S. Salcudean, “Dynamicnon-continuous virtual fixtures for operations on a beating heart using the da vinci R©research kit,” in The Hamlyn Symposium on Medical Robotics, pp. 45–46, ImperialCollege, London UK, 2015. → pages 67[65] S. A. Bowyer, B. L. Davies, and F. Rodriguez y Baena, “Active constraints/virtualfixtures: A survey,” Robotics, IEEE Transactions on, vol. 30, no. 1, pp. 138–157,2014. → pages 68[66] S. Nia Kosari, F. Ryde´n, T. S. Lendvay, B. Hannaford, and H. J. Chizeck, “Forbiddenregion virtual fixtures from streaming point clouds,” Advanced Robotics, vol. 28,no. 22, pp. 1507–1518, 2014. → pages 70[67] N. D. M. Sciences, “Optotrak certus,” 2015. → pages 79[68] N. Ho, “Finding optimal rotation and translation between corresponding 3d points,”2013. → pages 83119Appendix ARunning an ExperimentThis appendix has been included as an instruction set to the next poor soul who is takingon this project and wants to repeat the experiments. These instructions are for the userstudies described in Chapter 8, to test the second generation system. Before I start, mysincerest apologies, the system grew quickly in complexity near the end and I tried to keepit as neatly organized as possible. There may be better ways of doing it. Good luck!1. The first step: become well acquainted with the robot. I mean it. Develop a solidrelationship, get to know Dr. Jekyll and Mr. Hyde’s quirks and preferences. It’llhelp you both in the long run. This machine is pushing 20 years and new parts areno longer being produced. Take care of it. Spend some time looking at the dVRKSoftware Overview available from the User Meetings on the Intuitive dVRK UserGroup Website. Try playing with simpler examples first, like mainQtArm.cpp, main-QtTeleOperation.cpp and mainQtFollowTrajectory.cpp to figure out what’s going on.2. Deep breath, you’re ready to go3. Turn all the devices on(a) Turn on the da Vinci. Make sure all five arms are plugged into their dVRKcontrollers. Power the controller boxes in the order of the firewire daisy chain,top to bottom. Wait 10 seconds for the FPGA and QLA boards to initialize thenplug the firewire into the top of the daisy chain.(b) Turn on the NDI - this includes the large gray sensor as well as the sensor box.Make sure all the markers/strobers are plugged in as necessary.120(c) Turn on the Novinti. Plug it into the computer. This PC has the correctsystem requirements to run the Falcon.ii. If heaven forbid you plug a new Falcon in, re-install the drivers. Windowstries to be smart and automatically installs/overwrites the drivers whenyou plug in a new device, but these don’t work. You’ll hear an annoyingbuzzing sound from the computer if you try to communicate with the Fal-con. The executable to install the drivers is found in C:\ProgramFiles\Novintiii. Check to see that it’s working correctly with the Falcon Test utility thatcomes installed with the Falcon (there’s a link off the desktop), found atC:\ProgramFiles\Novint\Falcon\TestUtilities.iv. Run a trajectory, the LED lights will shine red until you manually movethe knob around to home the motors.v. Simulink models for the Novint and running the heart trajectory are inC:\Documents and Settings\Controller\Desktop\Heart motion trajectory\Novint PD ctrl.(d) Turn on the da Vinci Master Console and camera controller box.(e) Turn on all four computer involved: one for each of Novint (dragonfly.ece.ubc.caor DVRK SENSOR), NDI (, dVRK (tims-pc12.ece.ubc.caor DVRK PC1), and frame grabber (FRAME GRABBER PC, it’s got a colour-ful collection of cables hanging off it).4. Position the da Vinci slave arms by moving the setup joints as desired.5. Start the dVRK software. Launch mainQtTeleOperationVisualSevoing5.cpp applica-tion with the correct parameters.6. Find the optimal position for the phantom on the Novint on the patient side table.Make sure the camera can get a good view of it.7. Perform calibration between the NDI and the two robots(a) Launch the NDI streamer on the NDI computer(C:\NDIoapi\aruszkow\NDIOAPI svn\vc\NDIStreaming.sln(b) Open the stream on the dVRK side through the GUI(c) Make sure Falcon markers are in place and plugged into the strober121(d) Prepare the Falcon to run the calibration trajectory (run makeCalibTraj.m, thenset traj = makeCalibTraj.(e) Launch the Falcon to follow its calibration trajectory. Once it is initializedbefore it starts the calibration trajectory, “Run Falcon Calib” to start collectingthe streamed NDI data.(f) Once the Falcon is done, stop both the Falcon and the NDI stream by hit-ting “Run Falcon Calib” again. A calib NDI to Falcon.log should havebeen saved in your build directory.(g) Run saveData.m and commit the updated falconData.mat file to SVN. Updatethe corresponding SVN folder on the dVRK PC to have the Matlab Simulinksimulation data on the dVRK PC.(h) Unplug the Falcon markers and plug the dVRK markers into the NDI strober.Make sure the dVRK markers are in place, centered over O6.(i) Uncheck the teleoperated arms and make sure the correct calibration trajectoryis selected for the camera arm. Ensure the z-offset is correct. Run the dVRKcalibration.(j) Press “Run dVRK calib” again once the trajectory is done. Two more log filesshould have been generated.(k) Copy all pertinent log files (three named calib NDI ¡something¿ and the fal-conData.mat) to a desired location.(l) Run the mainAutoCalibration.m script in Matlab.(m) Rename the auto-saved T.txt in the rcltrans (see .bash aliases on thedVRK PC) folder to something meaningful.(n) Plug the Novint markers back into the strober and remove the dVRK markermount.8. Ready to start the actual test! Make sure the camera arm is well positioned and theoffset is updated.9. Select the teleoperation scaling factor to be consistent for both teleoperation modules(left and right).10. Might be a good idea to see if all arms can follow a simple z-sinusoid trajectory tocheck everything is set up correctly.12211. Prepare the BlackMagic Express capture on the Frame Grabber PC12. Make sure have no logs already (rm *.log in rclb)13. For each condition(a) Start with fresh suture, cut to half length. Explain they need to do at least 8sutures with it(b) Give user three practice sutures(c) Ask them to do five test sutures, all along one row on the target board. Give thema count down to start and enable logs in the dVRK GUI and start recording whatthey see in the frame grabber on “GO”. Make sure to stop recording and disablelogs once they say they’re done.14. Ask user to complete the Likert test15. Can use the SVN Matlab scripts I developed for data analysis (ReadCustomLog.m isthe best) as necessary, after the test.16. Clean up and power down in the opposite order of setup.17. All done, piece of cake! That wasn’t so bad :)123Appendix BSoftware OverviewFirst thing’s first, I must disclose I am a huge supporter of version control and was man-aging all the source code for the robot, as well as my personal code, using either GIT orSVN, through my Unfuddle account that’s managed by Tim.B.1 SVNI have asked for a repository for all my lab work and was granted: angelicar1/. Inside that repository I have some of my coursework, but more interestingly is the Thesis directory, which contains four subdirectories:1. HeartTrajectoryCode2. LogDataAnalysis3. NDICalibResults4. NDICalibration5. NDIOAPI svnMost of my code is well commented, and I’m a fan of ridiculously long variable names,because I’d often forget myself what my code was supposed to do. Hopefully it’s not toobad to read. Look for my README files, as well. I left a bunch around to again remindmyself what a particular script/test/file was for.124B.1.1 HeartTrajectoryCodeContains Matlab scripts for performing PCA, calculating FFTs, performing reconstruc-tions, and otherwise processing and manipulating the heart trajectory data.B.1.2 LogDataAnalysisContains a whole myriad of Matlab scripts that do all sorts of things. I tried to give thescripts clever names to remind myself what they do. Generally these scripts parse log filesproduced by the dVRK software, plot results, find transfer functions, extract critical points,and all sorts of fun helper functions. Most important is the ReadCustomLog.m script whichI have been solely using to read my .log files generated by the dVRK software. Easy tomodify, just change a few variables at the beginning and you’re good to go.B.1.3 NDICalibResultsThis is where the data from the Matlab Simulink simulation on the Falcon computer getsuploaded. Could’ve used a USB stick, this was easier.B.1.4 NDICalibrationContains the development, helper function, and main file for performing the calibrationbetween the Falcon and NDI, and NDI and dVRK to find the coordinate transforms betweenthe three elements. Start with main autoCalibration.m.B.1.5 NDIOAPI svnThis holds the source code for the Visual Studio project which uses the Optotrak API(OAPI) to stream the NDI measurements to the dVRK PC.B.2 GITFair warning: I’m going to assume here that the reader has basic experience with git.125The original open source CISST/SAW code is in a github repository found at Since there is a substantial amount of code that runsthe dVRK, and with multiple projects going on the lab I figured the best way to keep every-thing straight was just to divide all the code such that each project has its own git branch.In the main JHU github repository there are a number of submodules, essentially point-ers to other repositories, each containing a specific module. Pro tip: sawControllers andsawIntuitiveResearchKit are the one you’ll probably be modifying for your work. Thebranches are organized such that there is one master branch for the main repository, but 8branches in each of the submodules. If you ever want to switch branches, go to the topdirectory and use the script. Although most submodules aren’t used,it’s cleaner to just have all the submodules switch together, instead of remembering whichsubmodule is on which branch. The branches are1. master (Basically JHU’s code, with the addition of my followTrajectory code)2. dante (Co-op student’s surgeon training project)3. gaze (Irene’s gaze tracking code)4. movingECM (Omid and I tried to get the ECM to move, this is basically closed; JHUcame out with better code)5. simulink (Contains development and code for the Simulink controller project de-scribed in Section 5.3)6. virtualFixtures (Contains code for my work described in Chapter 77. visualServo (Contains code for the closed loop motion measurement system; basi-cally the second generation of the heart motion compensation system)8. waypointMode (Caitlin’s project to command the position of the dVRK through TCP/IPsocket communication from an ultrasound machine, coordinating moving a pickup ul-trasound probe with the dVRK and scanning the image with the ultrasound machine)The main dVRK machine is set up to pull from the JHU repositories and push to RCL’sUnfuddle repositories. This should be the only PC connected to JHU’s repository. All othermachines in the lab should deal exclusively with the RCL Unfuddle repositories. Instruc-tions on how to do that are found here:, I used to keep a detailed log when I was struggling through learning the dVRKcode, all on the RCL website, found here: a high level overview of the CISST/SAW architecture, read the dVRK SoftwareOverview available from the User Meetings on the Intuitive dVRK User Group Website.The documentation at, and are also hugely helpful. Worst comesto worst, there is a dVRK Google Group that is actually a wonderfully supportive commu-nity.127


Citation Scheme:


Citations by CSL (citeproc-js)

Usage Statistics



Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            async >
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:


Related Items