Calibration of Force/Torque and Acceleration for an Independent Safety Layer in Medical Robotic Systems

Background: Most medical robotic systems require direct interaction with the robot. ForceTorque (FT) sensors can easily be mounted to the robot. However, an accurate FT control requires the current robot position to compute the spatial orientation of the sensor for gravity compensation. Methods: We developed an independent safety system, named FTA sensor, which is based on an FT sensor and an accelerometer. With a calibration of accelerations to the FT coordinate frame, the current spatial orientation of the sensor is computed. Results: We found that the calibration of accelerations into the FT coordinate frame can be performed with a median rotational error of 3.5°. The median error for gravity compensation based on accelerations was 0.3N and 0.04Nm for forces and torques, respectively. Conclusion: By combining accelerations with force-torque readings, the FTA sensor works independently from robot input. Furthermore, the accuracy of the FTA sensor is sufficient for the purpose of medical robotic systems. Categories: Medical Physics


Introduction
Robotic assistance systems are more and more important for medical applications [1]. For recent developments on medical and surgical robotics see, e.g., [2][3][4]. Even though the applications and specifications differ, for many systems industrial robots are preferred instead of fully new designs [5][6]. The industrial robot is then adapted to the specific requirements of the applications. This adaptation is usually done in software, e.g. on the robot controller.
For Transcranial Magnetic Stimulation (TMS), a magnetic coil is placed on the patient's head for non-invasive brain stimulation. Recently, different robotic systems for TMS have been developed. A recent development introduces a specialized c-shaped robot design [14][15][16]. All other current robotic TMS-systems are based on industrial robots [17][18][19][20][21], including the commercially available TMS-robot SmartMove (Advanced Technology B.V., Enschede, The Netherlands).
For some medical robotic systems, force-torque sensors are mounted between robot end effector and tool [22][23]. These sensors are mainly used for haptic feedback, pressure control and/or user interaction. For accurate force and torque detection during operation, the tool's weight-related forces and torques must be subtracted. As this impact changes, depending on the spatial orientation due to gravity, the spatial orientation of the sensor must be known. Commonly, this is done by using the current robot end effector position. Besides additional latencies, the communication with the robot controller is mostly done in software and the computation is not independent of the robot [22]. Therefore, we propose an independent safety system that is easily integrable in the existing systems and adds an additional safety layer to these systems. It is based on a force-torque (FT) sensor which is FIGURE 1: Diagram of Robot combined with an inertial measurement unit (IMU). An embedded system instantaneously triggers the robot emergency stop in case of an error or collision. As the key feature, the embedded system provides gravity compensation independently from robot input in real-time using the acceleration recordings.
In this paper, we present the idea of combining acceleration measurements with an FT-sensor for independence from robot input. We also address the issue of calibration of IMU to FT sensor and briefly describe the system's setup and implementation. Besides evaluation of the calibration, we further show that the use of acceleration recording is sufficient for gravity compensation for medical robotic systems.

Common principle of gravity compensation
A force-torque (FT) sensor is capable of measuring forces and torques in the three spatial axes in real-time and high resolution. With such a sensor mounted to the robot end effector, we are able to detect impacts on the mounted tool. Due to gravity, the tool's weight affects the sensor. To measure and detect impacts, e.g., user interaction or a collision, with the sensor, we must compensate for the tool weight. By changing spatial orientation of tool and sensor, the influence of the weight on the recordings changes. Hence, we must consider the gravity compensation depending on the current robot orientation R ξ E . Accordingly, we must know the transform ETFT from robot end effector to the sensor. This principle is illustrated in Figure 1.
When we mount the tool to the sensor and record the current force F = (F x , F y , F z ) ′ , we can estimate the tool's zero force F 0 which is based on the force magnitude f = ǁFǁ 2 : Hence, we can calculate the expected force F ′ due to gravity and robot orientation: By subtracting F ′ from the current force recording F, we can calculate the applied force to the tool: In addition to forces, also torques due to gravity affect the recordings. For these torques M ′ and the tool's centroid s we have: We subtract M ′ from the current torque reading M to obtain the applied torque: To calculate the applied forces and torques the current robot orientation is required. In the presented method above, the robot orientation R ξ E is fed in from the robot [22,24].

Combining acceleration with force-torque
In contrast, an inertial measurement unit (IMU) can measure accelerations relative to gravity acceleration. Hence, the IMU is able to measure the gravity direction in relation to the IMU at rest. By combining such an IMU with an FT sensor, we can use the accelerations for gravity compensation. The combination of both sensors will be called FTA sensor. In contrast to FT sensors, IMUs are available as integrated circuits. As both, IMU and FT sensor, have their specific coordinate frame, we must perform a calibration between both the sensors. Thereby, ( ) 2012  we get the transformation matrix FT ξ IMU to convert the accelerations A from the IMU to the FT coordinate system: Now, we can use the accelerations to compensate for gravity. We calculate the expected force F0 for the current orientation with: We estimate the applied forces F and torques M corresponding to the above presented equations but with usage of Eq. (7) instead of Eq. (2). This way, robot input is not required for computing the spatial orientation of the sensor. Hence, it operates independently.
We use an embedded system (ES) for implementation of the calculations in real-time [25]. Figure 2 shows the communication setup for the embedded system. The ES directly reads the input from the IMU and the FT sensor and performs the calculations and triggers the robot's emergency stop in case of an error.

FIGURE 2: Communication setup
Communication setup. The embedded system reads data from the IMU and the FT sensor. It is connected to the emergency circuit via a relay. The embedded system provides a serial connection (RS-232) to the host system or robot. An optional USB connection is also provided. Furthermore, 2 additional I/O ports can be used for interaction.

Setup and circuit board
We use a K6D force-torque sensor (ME Systeme, Heringsdorf, Germany) and integrate the sensor into a specific casing. This casing allows for easy mounting to the robot end effector. Besides the FT sensor, the casing contains and protects the circuit board, including the IMU.
We mount the FTA sensor to an Adept Viper s850 industrial robot (Adept Technology, Inc., Livermore, CA, USA) as shown in Figure 3. We pass the communication and power supply cable through the robot's internal user communication interface. This way, intertwining of the cable with the tool or articulated arm is avoided.
The cicuit board consists of the IMU (LIS3LV02DQ; STMicroelectronics N.V., Amsterdam, The Netherlands) as the 3D accelerometer and a relay for connection to the emergency stop. The IMU is a three axes linear accelerometer with a measurement range of up to ±6G , with 1G = 9.81 m s 2 Furthermore, an analog-digital converter (ADC) is located on the board for reading the voltages from the FT sensor. Also, the board consists of a direct current converter for power supply. As microprocessor, we use an Atmel AT32 with a bandwidth of 32 bits and a processor clock rate of 60MHz.

Calibration of IMU to FT sensor
As IMU and FT sensor are located in the same casing, a coarse knowledge of their coordinate systems exists. However, for our application, an accurate transformation is required. Thus, a calibration of IMU to FT sensor is mandatory.
Once the FTA sensor is installed to the robot, we use a full circular motion in joint 4 of the articulated arm to perform calibration. For the circular motion, the angle values are used with the measured acceleration and Joint 5 is set to 45 ∘ to allow for non-zero measurements in all spacial axes. For

FIGURE 3: Equipment
The FTA sensor is mounted to an Adept Viper s850 robot. The communication channels are passed through the robots internal connections. The force-torque sensor is integrated in a casing which houses the circuit board with the IMU. The casing allows for easy mounting to the robot end effector.

FIGURE 4: Approximate spatial relationship between FT sensor coordinate system and IMU coordinate system
Approximate spatial relationship between FT sensor coordinate system and IMU coordinate system. calibration, we mount a weight to the FT sensor.
For each spatial axis and for each modality (force, torque, acceleration), we calculate a cosine fit using: In this case, the parameter c l describes the offset for forces, torques and accelerations. By comparison of the phase angle c l between forces F and accelerations A , we can compute the transform FT ξ IMU between FT sensor and IMU. As the translational shift of the IMU is meaningless, the transform only consists of a rotational matrix. Due to the system setup (cf. Figure 4), we have a coarse knowledge of the orientation of IMU and FT sensor: where → e denotes the corresponding unit vector. Figure 5 illustrates this relationship with recorded force and acceleration measurements. Also, the cosine fit for each modality is shown. Consequently, we know that a rotation of ≈ − 90 ∘ around the y-axis is needed to transform accelerations into the

FIGURE 5: Recorded (blue) and fitted (red) forces (upper row) and accelerations (lower row) during a full rotation of joint 4
Recorded (blue) and fitted (red) forces (upper row) and accelerations (lower row) during a full rotation of joint 4.
FT-sensor coordinate frame. Also, the other phase angles must be adapted, resulting in the following equation: where R y describes a rotation around the y-axis, R z and R x around z-and x-axis, respectively.
Using the phase angles b l , the equation can now be refined as: Note that the Equations (12) and (13) can be easily adapted to any other system setup. The rotational matrices must be changed in accordance with the specific setup. Also, we are using the calibration matrix C which converts the voltage readings from the FT sensor into forces and torques. As a result, we use to estimate the gravity compensated forces F user and torques M user , based on the voltage readings V , the accelerations A and the tool's gravity force F G and centroid s . Note that different methods for estimating the tool's weight and centroid exist, e.g. [24] for a TMS coil.

Calibration
First, we evaluate the accuracy of the calibration from IMU to FT sensor. Therefore, we perform the presented calibration method with two different FT sensors and two IMUs (including circuit board with ES), resulting in a total of four FTA sensors. For each FTA sensor, we perform three sets of calibrations with 20 calibrations in a 15-min-interval. We therefore have 60 calibrations ( ) ( ) of IMU to FT sensor for each FTA sensor that we use for evaluation.

Quality of the fit
As the calibration is based on fitted values (cf. Equation (8)), the quality of the fit is essential for the accurateness of the calibration. Therefore, we estimate for each recording of each modality the absolute distance to the fitted curve.

Calibration error
For calculating errors of the calibration, we first transfer the recorded accelerations A IMU into the FT coordinate frame by applying the computed transformation matrix FT ξ IMU (cf. Equation 6). We fit the transferred accelerations to a cosine with the formula from Equation (8). We compare the phase angles of the forces (estimated during calibration) to the phase angle of the transferred accelerations (A FT ) and compute the error for each spatial axis by applying the inverse sine to the phase difference.

Stability of calibration
For calculating the stability of the calibration, two calibration results T 1 and T 2 are used. To compare the difference between these two, we use and T e 2 = T 2 ⋅ T − 1 1 .
where T e i are rotational matrices. The stability is now expressed as the computed rotational error E rot as E rot = is necessary since the matrices T 1 and T 2 may be non-orthogonal. Consequently, since we do not wish to privilege one frame of reference, the average of the errors is used. This, and the way of computing the rotational error, is in line with standard approaches for hand-eye calibration [26][27]. Note that, as the calibration of IMU to FT only consists of a rotational part, no translational error is estimated.

Gravity compensation
To estimate the goodness of the independent gravity compensation based on accelerations, we | | | | mounted a weight to the sensor and estimated the tool's weight and centroid [24]. We used these parameters for gravity compensation (Equations (14) and (15)). We now moved the robot randomly within all spatial axes and recorded the gravity compensated forces and torques from the FTA sensor. In this way, we collected roughly 20;000 data points which we used for evaluation.

Calibration
Quality of the fit

FIGURE 6: Fitting quality
Quality of the cosine fitting used for calibration. The differences from recorded data to the fit are shown. From left to right: the results for forces, torques and accelerations are presented as boxplots. Figure 6 shows the overall cosine fitting quality used for calibration as boxplots. The median deviations for forces were 0:14N, 0:11N and 0:15N for the three spatial axes. For torques, it was 0:0034Nm, 0:0023Nm and 0:0017Nm, respectively. The median deviations for the accelerations were 0:016G, 0:027G and 0:022G, respectively. Due to noise, we were not able to perform a valid cosine fitting in two recordings. Therefore, these two recordings were excluded from further analysis.

Calibration error
The median calibration error, was 3.4 ∘ for the x-axis and 3.5 ∘ and 1.6 ∘ for the y-and z-axis, respectively. Figure 7 shows these results as boxplots.

Stability of the calibration
In total, we evaluated the stability of the calibration on almost 7,000 combinations of calibration results. Figure 8 shows the results as a boxplot. The median deviation was 0.89 ∘ . For the sensors 1 and 3, the median error was even below 0.7 ∘ . Figure 9 shows the error of the gravity compensated forces and torques. On average, the error for forces was in the range of 0.3-0.4N for each spatial axis. For torques, the average error was in the range of 0.02-0.045Nm. Note that the used weight corresponded to approximately 0.7Kg.

Discussion
We presented the use of acceleration measurements in combination with an FT sensor to perform gravity compensation independent from the robot. The necessary calculations for combining both sensors can be performed with an embedded system in real-time [25]. In this way, it acts as an independent safety-layer for medical robotics systems.
We have shown that the required calibration of the accelerations to the force/torque sensor coordinate frame can be done with a median error of roughly 3.5 ∘ . However, there have been some recordings with a larger fitting error due to noise in the measurements. As the calibration is only

FIGURE 8: Calibration stability
Stability of the calibration of IMU to FT coordinate frame as a boxplot. The errors for each used FTA sensor and the overall error are shown.
required once for each FTA sensor, we are able to repeat and extensively validate the calibration result. For instance, we can use the fitting error to validate if the quality of the measurements is poor. In case of noise, we will repeat the recordings to minimize the error. Therefore, it will be possible to perform a final calibration of the FTA sensor with a calibration error below 2 ∘ .
Our evaluation suggests that the presented calibration method produces stable results. The median deviation between two calibration matrices was 0.89 ∘ .
Besides these evaluations on the calibration itself, our practical test shows that the gravity compensation based on accelerations is sufficient for the application. The median error was roughly 0.3N for the force readings and approximately 0.03-0.04Nm for the torque readings. The maximum errors were below 1.25N and 0.13Nm for forces and torques, respectively.
For the robotized TMS system, the used contact pressure is in the range of 2-5N [22]. For user interaction with the robot using hand-assisted positioning, only forces larger than 2N and torques larger than 0.5Nm are taken into account to move the robot. Therefore, the presented gravity compensation is sufficient and applicable for the purpose of robotized TMS.
By combining accelerations and force-torque measurements with the data processing of a realtime embedded system, we can use the presented method for real-time monitoring of the robot.
In case of an error in the computation cycle, from reading of the measurements to the computation of the gravity compensated forces and torques, the FTA sensor can instantaneously trigger the robot's emergency stop [25]. The FTA sensor itself is easily mountable between robot end effector and tool. For safety monitoring, the used software does not have to be changed or adapted. The sensor runs independently from robot and software. Note that the presented gravity compensation based on accelerations also works in case the robot is positioned skewly (not aligned with the gravity). When