Toward Hybrid Force/Position Control for the Cerberus Epicardial Robot

Abstract

Gene therapies have emerged as a promising treatment for congestive heart failure, yet they lack a method for minimally invasive, uniform delivery. To address this need we developed Cerberus, a minimally invasive parallel wire robot for cardiac interventions. Prior work on Cerberus was limited to controlling the device using only position feedback. In order to ensure safety for both the patient and the device, as well as to improve the performance of the device, this paper presents work on enhancing the existing system with force feedback capabilities. By modeling the statics of the system and developing a tension distribution optimization technique, existing position control schemes were modified to a hybrid force/position controller. Experimental results show that using a hybrid force-position control scheme as opposed to position decreases positioning error by 38%.

I. Introduction

A promising topic in the field of cardiovascular research has been the use of gene therapies for congestive heart failure. Current practices lack effective ways to deliver a uniform distribution of gene expression that is required for myocardium interventions [1]. Ideally, this would entail a large number of small injections to cover a large area of the beating heart.

Traditional cardiac procedures involve opening the chest cavity to gain access to the paused heart and lungs. This exposes the patient to a risk of infection and longer recovery time [2]. Minimally invasive thorascopic techniques allow surgeons to reach the beating heart using rigid tools that are inserted between the ribs via small incisions. Thoracic procedures are limited by the trauma inflicted by deflating the left lung in order to reveal the heart, the need to stabilize the beating heart, and the rigidity of the tools that limits the workspace. Neither option provides an effective way for the delivery of gene therapy drugs.

Cerberus is a planar parallel wire robot developed for minimally invasive cardiac interventions. The device is inserted using a subxiphoid approach that accesses the heart while avoiding the lungs. Flexible arms then allow the device to expand into a triangular shape and adhere to the surface of the beating heart with suction at its three vertices, providing a stable platform with no motion relative to the heart. Wires from each base connect to an injector head that moves within the triangular support structure by changing the wire lengths. This design has the typical advantages of parallel wire robots, namely a large workspace and the ability to move quickly within this workspace [3]. These advantages give the device the potential to deliver multiple injections accurately over the entirety of the workspace to the beating heart.

Previous work on Cerberus has focused on adapting previously developed methods for parallel cable manipulators to our system [4]. Under simplifying assumptions about the geometry of the robot and neglecting the curvature of the heart, inverse kinematics that yield the wire lengths were successfully derived, and a control system was developed and tested in vivo using only position feedback.

With a surgical robot such as Cerberus, it is crucial that the forces produced by the robot are monitored and controlled to ensure safety. Such forces can be measured by the tensions in the wires under the assumption that the device is frictionless and non-inertial. Further, the wires can only exert force by pulling [3], [4]. Due to the device's actuator redundancy, the state equations for the forces in static equilibrium are coupled and nonlinear, leading to an infinite number of possible tension combinations. Hence, at a given point, the tension for each wire must be found by maximizing the number of wires that are within a safe range in the workspace. Limited work exists on finding tension distribution for planar cable-driven robots. While other parallel cable robots, such as the NIST ROBOCRANE [5], have the advantage of gravity to keep wires taut, Cerberus relies entirely on its actuators to maintain tension.

In this paper, state equations for statics are adapted from previously developed methods for one degree of actuation redundancy to fit this system [3] and a method to find the optimal tension distribution at a given point is developed [4]. Preliminary work is also done in adding force control to the existing position control that would confine tensions within an allowable range and increase position accuracy to make the device safer for surgery.

II. Methods

A. System Hardware

The previous control system [6] was adapted to fit three load cells using a pulley system and calibrated to measure the tension in each wire. A profile view of the system can be seen in Fig. 1. For the purposes of this experiment, a desktop setup was designed capable of fixing the three bases of the robot to a planar surface while allowing variation the lengths and angles of the arms at known values as shown in Fig. 1. A Pixy camera was mounted directly overhead to capture all possible configurations within the camera's field of view. Ground truth was established using the camera's color tracking software via markers on the bases and injector.

Fig. 1.

(Top) Control system in acrylic box. Each servo rotates a reel that tensions the corresponding wire. The wires extend across pulleys to a corresponding load cell. (Bottom) Desktop setup with three bases fixed on a platform, where the left and right base can be set to various lengths, and a mounted camera.

B. Kinematics

Kinematic equations were adapted to fit this system from previous work for general parallel wire manipulators with one degree of actuation redundancy [3]. No closed-form solution exists for the forward kinematics because the system is a parallel manipulator. Inverse kinematics can be found by drawing concentric circles around each base that intersect at the injector to find the Euclidean distance between each base assuming that the middle base is set as the origin under Cartesian coordinates (Fig. 2a). The lengths of the wires are given by [4]:

Fig. 2.

(a) Ideal kinematics and (b) the free-body diagram of the planar wire robot manipulator.

$$\left[\begin{array}{c}{r_l}^2\\ {r_m}^2\\ {r_r}^2\end{array}\right]=\left[\begin{array}{c}{(x_0+L_{l}cos{\theta }_l)}^2+{(y_0-L_{l}sin{\theta }_l)}^2\\ {x_0}^2+{y_0}^2\\ {(x_0-L_{r}cos{\theta }_r)}^2+{(y_0-L_{r}sin{\theta }_r)}^2\end{array}\right].$$ (1)

C. Statics

Previously developed methods for parallel wire robots with one degree of actuation redundancy [3] have been adapted for Cerberus. It is assumed that the mass of the end-effector or injector is negligible. Then, even in motion, the system can be modeled such that the sum of the wire tensions will always be zero. A free-body diagram of this static model is shown in Fig. 2b. The Cartesian coordinates are altered from the frame used in kinematics (x, y) with the middle base as the origin to (x′, y′), where the injector is now taken as the origin such that the middle wire lines up with negative y′, with a similar transformation for the angles (Fig. 2b). Resulting static equations are given by:

$$\sum F_{x\prime }=t_{l}sin{\varphi }_l+t_{m}sin{\varphi }_m+t_{r}sin{\varphi }_r=0$$ (2a)

$$\sum F_{y\prime }=t_{l}cos{\varphi }_l+t_{m}cos{\varphi }_m+t_{r}cos{\varphi }_r=0$$ (2b)

where t_l, t_m, t_r are the tensions applied by the left, middle, and right wires respectively. This can also be expressed as:

$$\begin{array}{l}\mathbf{S}\mathbf{T}=\mathbf{0}\hfill \\ \mathbf{S}=\left[\begin{array}{ccc}sin{\varphi }_l& sin{\varphi }_m& sin{\varphi }_r\\ cos{\varphi }_l& cos{\varphi }_m& cos{\varphi }_r\end{array}\right]\hfill \\ \mathbf{T}=\left[\begin{array}{c}{t}_l\\ t_m\\ t_r\end{array}\right].\hfill \end{array}$$ (3)

Due to the actuation redundancy, (3) is underconstrained, meaning there are infinitely many tension combinations that would satisfy the equation.

D. Optimal Tension Distribution

In order to solve for the tensions in the system, T, (3) must be inverted. Then the tensions of the wires in our system can be expressed as:

$$\mathbf{T}=\alpha \mathbf{N}\mathbf{,}$$ (4)

where N = [n_l n_m n_r]^T is the kernel vector of the components of the tensions from (3) and α is a scalar weight. A method to calculate the kernel vector was adapted from [3], where each component is found by taking the determinant of the 2×2 submatrix of S with the corresponding column removed. Using this method the kernel vector for this system would be:

$$\mathbf{N}=\left[\begin{array}{c}{n}_l\\ n_m\\ n_r\end{array}\right]=\left[\begin{array}{c}sin({\varphi }_m-{\varphi }_r)\\ sin({\varphi }_r-{\varphi }_l\\ sin({\varphi }_l-{\varphi }_m)\end{array}\right].$$ (5)

By setting the coordinate system such that ϕ_m = 180°, equation (5) can be simplified to:

$$\mathbf{N}=\left[\begin{array}{c}sin({\varphi }_r)\\ sin({\varphi }_r-{\varphi }_l)\\ -sin({\varphi }_l)\end{array}\right].$$ (6)

The kernel vector given by equation (6) gives us the ratios of tensions that satisfy equation (3). Recall that parallel wire robots can only exert tension. Thus, the tensions in the wires T will always be positive in the workspace. The signs of the tensions are expressed by the trigonometric expressions in N, where a point outside of the workspace will result in an angle that produces a negative n and a point taken on the edge of the workspace will result in n = 0.

The tensions are also constrained by T_min ≤ T ≤ T_max, where T_min > T will result in a loose wire that could get caught on something and T > T_max may snap the wire, break the robot, or restrict heart movement [6]. To ensure that this is satisfied by each of the wires, the scalar α must be chosen such that they all meet the minimum tension requirement for any given point within the workspace. This can be generalized as:

$$\alpha =\frac{T_{\mathit{min}}}{min\mathbf{N}}.$$ (7)

Combining (4), (6), and (7), the optimal tensions required to keep the injector static can be found for any point within the workspace. Note that tensions above the maximum allowed force must be manually rejected.

E. Hybrid Force/Position Control

Due to the device's actuation redundancy, each wire must be controlled separately via servos. Prior controls relied on varying individual wire lengths, calculated using inverse kinematics based on the desired position and the encoder values of the servos translated into lengths, to move the injector. Hence inaccurate calculation could cause errors in position, allowing wires to become too loose or too tight. Such occurrence would translate to the surgeon having poor control of injection placement in an application setting. A hybrid of force and position control would give the best of both scenarios; allowing for accurate position of the injector, determined by wire length and optimal tensions, while wire tensions are maintained within the proper range.

A parallel controller was implemented using two PID controllers using desired location (Fig. 3). The force control branch used force feedback as the input with the set point based on the calculated desired tensions for the desired target location. The position control branch used position feedback as the input with the set point based on the calculated desired wire lengths for the desired target location.

Fig. 3.

Control diagram for hybrid force/position control.

Movements of the injector were divided into stages. After a desired position is commanded via the surgeon, the robot calculates the desired wire lengths and optimal tensions. In the first stage of the motion, both position and force control are given equal weights in moving the robot. As the injector closes in one the desired wire lengths within 1 cm of the desired wire lengths, position control is weighted heavier than force control until the wire lengths are within a 2 mm of desired values. After which, force control becomes weighted more alters the wire lengths until the desired statics are achieved.

This hybrid force/position controller was tested against the controller that used only position feedback for various geometric configurations. The injector was sent to ten desired locations within the workspace. These points were compared to the injectors' actual location obtained from a camera via color markers. The resulting points were compared to the desired locations to find error and covariance.

III. Results

Simulations and bench top experiments were conducted to verify the validity of tension measurements and the hybrid controller under operational conditions.

A. Optimal Tension Distribution

The device was simulated using MATLAB to assess the dependence of the workspace with regard to geometry and tension limits using the optimal tension distribution algorithm as discussed previously.

The simulation was able to calculate the optimal tension distributions with various arm lengths and angles, validating that the technique is robust against any geometry. An example of such a distribution is demonstrated in Fig. 4, where the color map indicates the minimal tension required by the wires within the workspace as a ratio between the set maximum and minimum tensions. It was found that the lowest minimum tensions, shown in blue, were concentrated in the middle of the workspace while the highest minimum tensions, shown in red, were concentrated on the edge. The lack of color around the arms of the robot indicates that tensions in those areas could not satisfy the constraint, namely that the larger tensions were above the maximum. Therefore, the amount of usable workspace depends on the ratio of maximum to minimum tension, where increasing the maximum tension will increase the accessible workspace. This technique can be used to determine the geometry that optimizes the amount of usable workspace.

Fig. 4.

Contour graph of tension optimization under the geometry of L_l = 100 mm, L_r = 100 mm, θ = 30°, T_min = 100 mN, and T_max = 500 mN.

B. Hybrid Force/Position Control

The controller that uses desired wire lengths and tensions to move the injector as shown in Fig. 3 was implemented. Fig. 5 shows the time trial results of hybrid controller of desired values and actual values showing each wire achieving desired tension within 20 seconds. To reach this tension, each wire had to deviate from its desired length value. This may be due to inaccuracies in the calculation for the wire length due to simplification of the system. The stages of movement can be seen in Fig. 5 where, within 10 s after starting, the arm lengths diverge from their desired values to compensate for tensions.

Fig. 5.

Experimental arm lengths and tension distribution results when injector is commanded to go to center of workspace for one sample run. Commanded values are shown in red, and experimental measurements in blue. Injector is assumed to start at middle base where the left and right wires are fully extended. Negative wire lengths indicate that the wire must retract.

The injector's position accuracy was tested using hybrid force/position control and position-only control. The injector was commanded to go to ten known positions. Ground truth of the end-effectors was found using the built-in color tracking system of a Pixy camera fixed above the platform setup. Arm lengths and angles were varied. An example of an experiment is seen in Fig. 6. Analysis of global accuracy reveals that hybrid control has proportional error for both x and y position error while position control has significantly higher error in the x-direction. Overall, the hybrid controller yielded positioning error of 1.8 ± 0.8 mm, while position control yielded error of 2.9 ± 1.4 mm.

Fig. 6.

Local (top) and global (bottom) point positioning results from 10 points (black) within workspace using hybrid (blue) or position (red) controller with the geometry of L_l = 100 mm, L_r = 100 mm, θ = 30°, T_min = 100 mN and T_max = 500 mN.

IV. Discussion

Current models of the device are simplified for ideal conditions. Future work must focus on more realistic geometry. In particular, taking into account the base diameter will change both the wire length and angles used to calculate optimal tensions.

The next step for this robot would be to translate the algorithm from the current planar model to a curved surface to replicate more realistic heart conditions. This would begin on an idealized sphere and then move onto irregularly curved surfaces similar to the heart. Finally, the movement of the heart should be taken into account so that the wires will adjust to move with the beating heart as to maintain constant tensions in the wire without moving the injector.

Force measurements of a parallel wire manipulator with actuation redundancy for cardiac interventions were successfully integrated with the pre-existing system using load cells. The capability of monitoring tensions of wires ensures the safety of the patient by ascertaining that the forces are within a range that allows the surgeon to maintain control of the injector's placement by keeping wires taut yet low enough to maintain the integrity of the robot as well as to not interfere with heart movement. The additional information about the system state also means that desired injector position potentially can be more accurate.