Contact Us
CiiS Lab
Johns Hopkins University
112 Hackerman Hall
3400 N. Charles Street
Baltimore, MD 21218
Directions
Lab Director
Russell Taylor
127 Hackerman Hall
rht@jhu.edu
Last updated: 05/09/20 5:00pm
The goal of the project is to develop a human arm joint angle measuring system that captures up to 6 DOF which serves as a portable alternative to the master surgeon console in teleoperation surgery, for example, the Master Tool Manipulator (MTM) of Intuitive Surgical’s da Vinci system. In particular, the system must be fully wearable, have similar workspace as the MTM, and recognize the surgeon's intention to engage/disengage with the system (referred to as rules of engagement below). In short, the goal of this project is to develop a mobile telesurgery interface with the da Vinci Patient Side Manipulator (PSM).
The current da Vinci surgeon console is stationary in the Operating Room (OR) and is placed in the non-sterile field of the room. This means that the surgeon performing the teleoperation surgery is not able to perform any operation on the patient bed, which consequently mandates another surgeon or first assistant to be present at bedside to assist changing instruments of the da Vinci patient cart and any other necessary operations for the surgery. From Intuitive Surgical’s feedback upon the original technical proposal submitted by the mentors listed in this project, it is clear that they desire a system that the current surgeon console cannot achieve, one benefit of which being able to perform solo surgery. Economically, the surgeon console is costly to manufacture. Lowering the cost of the capital equipment required to deliver the robotic instruments to the patient would benefit both hospitals, by reducing the up-front cost of the system, and Intuitive Surgical, by enabling the company to focus on the instruments [1]. Apart from the proposed system described in the previous section, the mentors mentioned in this proposal had a large amount of research on a well-developed Head Mounted Display (HMD) system that can display the endoscopic image, being an alternative to the stereoscopic display located on the surgeon console. The grand goal of this project is to develop a novel surgeon console that can be sterile, mobile and lower cost than the current surgeon console, which will provide the benefit of solo surgery, surgery collaboration between more than 2 surgeons, and easier introduction of the system to hospitals worldwide.
Overview
Fig 1. System Diagram
The project system diagram is shown in Figure 1, which consists of hardware and software. The communication between hardware and software components are by standard class 2 Bluetooth host interface [1]. Once the software receives real-time (100Hz) IMU data, the kinematics algorithm developed by me calculates the joint angles and the forward kinematics, then controls a fully rigged avatar, as shown in the right of Figure 1. The joint angles computed by the algorithm is translated to the avatar’s joint angle, which fairly represents the motion that the system user is making, thus achieves joint space control; the forward kinematics computed by the algorithm controls a sphere in the Unity scene that represents the end effector global position and orientation, which gives the system the capability to achieve cartesian space control.
In order to implement the system and achieve joint space control, an algorithm of transforming IMU reading to human joint angles is implemented, as well as a calibration protocol for IMU rotation. In order to achieve cartesian space control, a forward kinematics algorithm is implemented, as well as a calibration protocol to find the arm lengths.
Hardware
The hardware component consists of two 9-axis Bluetooth IMU, LPMS-B2 by LP-RESEARCH Inc.. The IMU has integrated Kalman Filter sensor fusion algorithm that fuses data from the 3-axis gyroscope, 3-axis accelerometer and 3-axis magnetometer. Users have the freedom to choose from Gyroscope only, Gyroscope + accelerometer, and Gyroscope + accelerometer + magnetometer modes[1]. In this system setup, I chose the Gyroscope + accelerometer Kalman Filter, since the magnetic field disturbance is high in my particular workspace, thus making the Yaw-axis drift much more significant than what is desired. Hardware setup
Fig 2. Hardware setup diagram
In the system setup, the 2 IMUs are placed strategically on the user’s arm, to ensure a good correlation factor from the IMU reading and the actual user joint angles. The first IMU is placed on the upper arm, medial head side of the tricep; the second IMU is placed on the extensor retinaculum. Human Arm Kinematics
Fig 3. Human arm kinematics
TSB=[R,0](1)
The translation of this homogeneous is 0 because in the system setup, the shoulder frame is at the origin of the world frame. The R here is the rotation matrix of the shoulder joint.
TES=[Rz,p](2)
The translation vector p here is [0, lu, 0]T, where lu is the length of the upper arm. The Rz here is the rotation matrix around z-axis and z-axis only.
TWE=[Ry,p](3)
The translation vector p here is [0, lf, 0]T, where lf is the length of the forearm. The Ry here is the rotation matrix around y-axis and y-axis only.
vPW=[0,lp,0]T(4)
This is a vector which indicates the local coordinate of the middle of the palm wrt the wrist local frame. In order to know the world coordinate of the middle of the palm vp(end effector), simply multiply (1) through (4):
vp=TSB*TES*TWE*vPW(5)
Calibration protocol
Rotation calibration
Fig 4. IMU’s body-fixed frame diagram [1]
IMU’s internal +y should be perpendicular to the workspace(screen). IMU’s internal -z should be pointing toward the ground. If IMU drifts during operation, perform “heading reset” [1].
Link length calibration
Fig 5. Link length calibration workspace
A 2D calibration object is defined. The object is placed on a level tabletop, with four calibration posts located in the four corners of the object. The dimension of the calibration object is defined in Figure 5. A brute force calibration method is used in MATLAB (calibration.m). The math concept is as follows:
d =actual distance between calibration posts(6)
e = distance between two end effector position when placed on two calibration posts(7)
In order to find e, first the user needs to align his/her palm on the different calibration posts, and then record the transformation matrix outputted by the software(unity); secondly, input all the matrices into the MATLAB code, which computes the position of the palm in world space. Then, e is obtained by calculating the absolute difference between palm position.
It is important to note that, the lf and luin the forward kinematics mentioned in Page 4 of this report are the variables to be calibrated. As a result, the brute force calibration searches for the values of these two variables that minimizes:
d-e(8)
Software
Fig 6. Unity software scene overview
A real time visualization scene is set up in Unity, where a mannequin’s right arm is controlled by the 2-IMU system worn by the user.
Rules of engagement protocol
Rules of engagement is the indication of the user’s desire to engage or disengage with the system, analogous to the clutch pedal on da Vinci’s surgeon console. The protocol is defined as follows: If the user rotates his wrist two times in a quick succession, the system switches from disengage to engage, or vice versa.
El-Gohary, M., & McNames, J. (2012). Shoulder and elbow joint angle tracking with inertial sensors. IEEE Transactions on Biomedical Engineering, 59(9), 2635–2641. https://doi.org/10.1109/TBME.2012.2208750
Naidu, D., Stopforth, R., Bright, G., & Davrajh, S. (2011). A 7 DOF exoskeleton arm: Shoulder, elbow, wrist and hand mechanism for assistance to upper limb disabled individuals. IEEE AFRICON Conference, (September), 1–6. https://doi.org/10.1109/AFRCON.2011.6072065
IMU's official website: https://lp-research.com/9-axis-bluetooth-imu-lpmsb2-series/
IMU's code base: https://lp-research.com/support/
IMU's Unity Plugin: https://bitbucket.org/lpresearch/openzenunity/src/master/
