Parallel Force/Position Control of an Epicardial Parallel Wire Robot

Abstract

Gene therapies for heart failure have emerged in recent years, yet they lack an effective method for minimally invasive, uniform delivery. To address this need we developed a minimally invasive parallel wire robot for epicardial interventions. Accurate and safe interventions using this device require control of force in addition to injector position. Accounting for the nonidealities of the device design, however, yields nonlinear and underconstrained statics. This work solves these equations and demonstrates the efficacy of using this information in a parallel control scheme, which is shown to provide superior positioning compared to a position-only controller.

I. INTRODUCTION

GENE therapy has emerged as a promising treatment for congestive heart failure [1]–[3]. However, the current state of the art lacks effective ways to deliver a uniform distribution of gene expression that is required for myocardium interventions [4]–[6]. Ideally, this would entail a large number of small injections to cover a large area of the beating heart.

Cerberus, shown in Fig. 1, is an organ-mounted 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. 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 rapid motion [7], combined with the demonstrated advantages of organ-mounted robots, which include a platform of zero relative motion with the heart [8], [9]. These advantages give the device the potential to deliver multiple injections accurately over the entirety of the workspace to the beating heart.

Fig. 1.

Cerberus parallel wire robot for rapid accurate myocardial injection for gene therapy. The prototype is deployed on porcine heart in situ.

Previous work on Cerberus [10]–[12] has focused on adapting existing methods for parallel cable manipulators to our system [13]. Assuming ideal geometry, inverse kinematics that yield the wire lengths were successfully derived, and a control system was developed and tested in vivo using only position feedback [12]. As Cerberus will be operating on a living heart, it is crucial that the internal forces produced by the robot are monitored and controlled to ensure safety of the patient. Previous studies show it is important to control both wire tension and position, ensuring higher accuracy of the end effector as well as maintaining safe tensions [11].

In order to provide more accurate tool positioning and wire tension, this letter presents the full kinematics and statics solution for the Cerberus robot. Non-zero radii of the suction bases that redirect the wires are accurately modeled, as are the non-zero offset distances of the wire connection points on the injector head. Inclusion of these offset distances necessitates including moments in the statics analysis, and a state variable for the tool rotation, β, must be included also. As a system having an equal number of actuators and degrees-of-freedom (DOF), known as an incompletely restrained positioning mechanism (IRPM) [14], in order to have non-zero wire tensions the device must be in a persistent singular configuration.

While devices such as the NIST ROBOCRANE [15] have an equivalent number of actuators to DOFs (6), gravity acts as a seventh “wire” to completely constrain them. IRPMs have been studied previously [16], [17]; however, these crane-like devices use gravity to passively maintain wire tension. To our knowledge, this work is the first to study a planar IRPM for a surgical application. In this work we present the full inverse kinematics and statics of Cerberus, identify the singular configurations, and use the improved kinematics and statics solutions as control inputs to a parallel force/position control scheme. Results demonstrate improved positioning accuracy.

II. METHODS

A. System Hardware

The previous control system [12] was adapted to fit three load cells using a pulley system and calibrated to measure the tension in each wire, as shown in Fig. 2(b) and Table I. 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 of the lengths and angles of the arms at known values as shown in Fig. 2(a). A Pixy camera (Charmed Labs, Austin, TX) 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. The camera is only used to quantify error, not in the actual control system. The camera was calibrated using the Matlab Camera Calibration Toolbox and has an uncertainty of 0.17 mm in each direction.

Fig. 2.

(a) Planar robot setup with colors for tracking. Green, yellow, and blue correspond to index numbers 0, 1, and 2, respectively. The red color covers the tool. (b) Control box with Arduino Mega 2560, continuous rotation servos, rotary encoders, load cells, and pulleys.

TABLE I.

Control Hardware Components

Part	Vendor/PN	Specifications
Load Cell	RobotShop - 3133-CZL635	5kg Wheatstone Bridge
Motor	Parallax-900-00008	0-50RPM Continuous-Rotation Servo
Encoder	Bourns - PEC12-4225F-N0024	96 Counts Per Revolution

B. Inverse Kinematics

Prior implementations of the inverse kinematic equations ignored the effects of the radius of the distal bases, which act as pulleys to redirect the wire to the injector head, and the connection points of the wire not being coincident at the center of the injector head. These non-idealities can be seen in the kinematic diagrams of the device in Fig. 3. Ignoring these relatively small values reduces the inverse kinematics to calculating distances between known points.

Fig. 3.

Geometry and variables describing the inverse kinematics.

Accounting for the connection points of the wires to the injector head, shown in Fig. 3, introduces a new state variable to account for the rotation of the injector head, β, which, because the wires can only provide tensile force, is not a free variable. Therefore, in order to solve for the wire lengths required to reach a desired position, we must now solve the coupled statics equations. The remainder of this section outlines the inverse kinematics of the device, while the following two sections address the statics and solving the coupled system of equations.

The inverse kinematic equations give the required wire lengths, w, to place the injector at a desired pose, (x, y, β), in the robot workspace. The base frame of the robot, b, is defined such that it coincides with the center of the proximal suction base with the y-axis bisecting the angle between the left and right arms, while the tool frame of the robot, t, is defined to coincide with the center of the injector head, with the y-axis defined to align with the centroid of the injector and the connection point of the middle wire, as shown in Fig. 3. The wire lengths, w, are then given by:

$$w=\left[\begin{array}{c}{w}_0\\ w_1\\ w_2\end{array}\right]=\left[\begin{array}{c}{r}_b{\alpha }_0+\Vert d_0\Vert \\ \Vert d_1\Vert \\ r_b{\alpha }_2+\Vert d_2\Vert \end{array}\right],$$ (1)

where r_b is the radius of the distal bases, α_i is the angle through which the wire wraps on the distal bases, and d_i are the vectors from the wire connection points to the tangent locations on the distal pulleys and the center of the proximal base. Left and right wire lengths are defined from these locations because the point at which the wires leave the pulley will change throughout the workspace.

The wire vectors, d_i, are given by:

$$d_0=b_0+r_{t,0}-h_0^b$$ (2)

$$d_1=-h_1^b$$ (3)

$$d_2=b_2+r_{t,2}-h_2^b,$$ (4)

where b_i are the arm vectors, r_t,i is the vector from the center of the distal base to the tangent point, and $h_i^b$ are the locations of the wire connection points with the tool head in the base frame. These locations are calculated by transforming the locations in the tool frame, $h_i^t$, by the tool pose, (x, y, β).

$$h_i^b=R_t^b{h}_i^t+\left[\begin{array}{c}x\\ y\end{array}\right]$$ (5)

$$R_t^b=\left[\begin{array}{cc}\text{cos}\beta & -\text{sin}\beta \\ \text{sin}\beta & \text{cos}\beta \end{array}\right]$$ (6)

The base vectors are calculated considering the arm lengths and angle between the arms:

$$b_0=a_0\left[\begin{array}{c}-\text{sin}\theta \\ \text{cos}\theta \end{array}\right]$$ (7a)

$$b_2=a_2\left[\begin{array}{c}\text{sin}\theta \\ \text{cos}\theta \end{array}\right]$$ (7b)

where a₀ and a₂ are the left and right arm lengths, θ is half the angle between the two arms. r_t,i is straightforward to calculate through trigonometry:

$$r_{t,0}=\left[\begin{array}{c}r\text{sin}{\gamma }_0\\ r\text{cos}{\gamma }_0\end{array}\right]$$ (8a)

$$r_{t,2}=\left[\begin{array}{c}-r\text{sin}{\gamma }_1\\ r\text{cos}{\gamma }_1\end{array}\right]$$ (8b)

where γ_i is the angle of the tangent radius vector with y^b. This angle is calculated by using the fact that the tangent point is perpendicular to one radius vector, providing the relationship:

$${\gamma }_i=\pi -{\text{cos}}^{-1}\left({\tau }_i\right)-{\text{tan}}^{-1}\left({\epsilon }_i\right)i=0,2$$ (9a)

$${\tau }_i=\frac{r}{\Vert b_i-h_i^b\Vert }i=0,2$$ (9b)

$${\epsilon }_i=\left|\frac{{\left(b_i-h_i^b\right)}_x}{{\left(b_i-h_i^b\right)}_y}\right|i=0,2$$ (9c)

C. Statics

Due to the non-zero magnitudes of the wire connection points, we must now consider moments in the statics analysis. A free-body diagram of the injector head is shown in Fig. 3. Summing the forces and moments about the injector head in the base frame yields the following system of equations.

$$\mathbf{S}\mathbf{t}=0$$ (10a)

$$S=\left[\begin{array}{ccc}\widehat{d_0}& \widehat{d_1}& \widehat{d_2}\\ \widehat{d_0}\times R_t^b{h}_0^t& \widehat{d_1}\times R_t^b{h}_1^t& \widehat{d_2}\times R_t^b{h}_2^t\end{array}\right]$$ (10b)

Where t = [t₀t₁t₂]^T are the tensions applied by the left, middle, and right wires respectively and $\widehat{d_i}$ are the normalized wire vectors. Due to the inclusion of the moment equation that must be satisfied, the force Jacobian matrix, S, is now square (3 × 3) (the bottom part consists of column vectors). While the statics equations are written in linear form for clarity, the inclusion of the unknown tool head rotation, β, in both the wire vectors and rotation matrices does not allow for a simple linear solution.

D. Optimal Tension Distribution

The system of equations which must be satisfied for static equilibrium, Eq. (10a), is underconstrained, with three equations in four unknowns. However, this decouples into a nonlinear problem in β and a linear system to solve for the tensions.

The tensions, t, which satisfy Eq. (10a), lie in the null space of S. Existence of a non-trivial solution to (10a), one in which the tensions are not zero, requires S to have rank < 3, and for any reasonable geometry must be 2. This drop in rank corresponds to the loss of a degree of freedom putting the system in a singular pose. This fact means that the rotation of the tool head is uncontrollable, and for the device to be in static equilibrium must always be in a singular pose.

A plot of tool orientation versus the value of the smallest eigenvalue of the matrix S for a particular location in the workspace is shown in Fig. 4. A drop in matrix rank corresponds to an eigenvalue of zero, which occurs at only one reachable orientation for each position in the workspace. The tension distribution, up to scale, is then the corresponding eigenvector for this value of β.

Fig. 4.

Plot showing how the minimum of abs (e) changes with β, approaching zero at only one point. In this case, β = 8.07° at (20,95).

We must also at this time consider the constraints we wish to apply on the forces in the system. While the design of the robot itself only allows for tensile forces, we wish to further constrain these forces to ensure accurate positioning as well as safety. Allowing a wire to go slack (t_i = 0) disrupts the ability to accurately control the position of the tool, while forces which are too large may result in loss of traction, destruction of the robot, or dangerous compression of the heart. Using the constraints

$$t_{\mathit{\text{min}}}\le {\mathrm{t}}_i\le t_{\mathit{\text{max}}}i=0,1,2$$ (11)

we can find which positions in the workspace are actually accessible, given the null space solution.

As the system is redundantly actuated, any scalar multiple of the solution will also be a solution. The “optimal” tensions in this case, are found by minimizing the total sum of tensions, which is achieved using the following metric.

$$t_d=\frac{t_{\min }}{min\left(t\right)}t$$ (12)

It is important to note here that any point in the workspace that returns a negative tension will lie outside the convex hull of the support triangle, and any point in which Eq. (11) is not satisfied, while lying inside this polygon, is not in the reachable workspace.

E. Parallel Force/Position Control

Accurate and safe operation of Cerberus requires two control objectives: accurate tool placement in the workspace requires position control, while ensuring the device does not damage itself or the heart requires accurate control of wire tensions. Neither of these objectives is dominant, making ill-suited an admittance scheme, which imposes position, or an impedance scheme, which imposes forces.

In order to simultaneous control both wire tension and lengths we implement a parallel force/position control scheme [18], shown diagrammatically in Fig. 5. Control of the device is done in joint space. Desired Cartesian location is used to calculate the required wire lengths and tensions. While the overhead camera in the experimental setup would allow us to control directly in the workspace coordinates, this measurement is currently not available to our system clinically.

Fig. 5.

Control diagram for parallel force/position control.

This parallel control method possesses the simplicity of impedance control, the force/position control abilities of parallel force/position control, and takes advantage of position and force measurements [19]. In our implementation we use only a proportional term for tension, and both a proportional and integral term for wire length. Derivative terms were not included due to noise. Gains used are shown in Table II. The input to the motors uses pulse-width modulation.

TABLE II.

Controller Parameters

Controller	Constant	Wire	Value	Units
Force	P	0	0.03	$\frac{\text{Duty Cycle}\%}{g}$
		1	0.03
		2	0.04
Position		0	0.52	$\frac{\text{Duty Cycle}\%}{\text{Degree}}$
		1	0.64
		2	0.29
	I	0	1.32	$\frac{\mathrm{s}\times \text{Duty Cycle}\%}{\text{Degree}}$
		1	1.82
		2	1.03

III. RESULTS

In order to validate the previously derived inverse kinematics and statics solutions they were first implemented in simulation. Minimum and maximum wire tensions were set to 1 N and 2 N, respectively. A maximum tension of 2 N means that the maximum force a suction base would see is 4 N. Previous studies of organ-mounted robots have safely demonstrated traction forces up to 4 N on the porcine heart [8], [9]. Two different robot geometries were used: one with equal length arms of a₀ = a₂ = 100 mm and an angle θ = 60°; the other with arms of unequal length where a₀ = 100 mm, a₂ = 125 mm, and θ = 60°. These two geometries are referred to by the ratios of their arms: 1:1 and 1:1.25, respectively. Multiple geometries were included to determine the effect this asymmetry has on the system, as initial prototypes of the device require asymmetric arms to fit easily through a cannula and to sufficiently cover the left ventricle.

Following the validation of the inverse kinematics and statics in simulation, the proposed parallel force/position control scheme was tested on the planar benchtop Cerberus prototype.

A. Orientation and Tension Distribution

Surface plots of the orientation and tension distribution results for both robot geometries are shown in Fig. 6. Note that any portion of the plot that is not colored falls outside of the reachable workspace of the robot, even though it may fall within the robot support structure. This reduction in workspace is due to the constraints on wire tension.

Fig. 6.

Orientation surface plots for (a) 1:1 geometry and (b) 1:1.25 geometry, with t_max/t_min plots for (c) 1:1 geometry and (d) 1:1.25 geometry.

For the symmetric robot, the rotation of the tool is symmetric about the y-axis, as expected, with maximum rotation of approximately ±0.4° [Fig. 6(a)]. The rotation of the tool head of the asymmetric robot, shown in Fig. 6(b), however is less intuitive with rotation ranging from approximately 0° in the bottom portion of the workspace and increasing to 8° in the upper portion of the workspace.

Plots showing the ratio of tensions, (max (t)/min (t)), for both geometries are shown in Fig. 6(c) and (d). Once again the plot for the symmetric robot is symmetric about the robot y-axis as expected. At the center of the workspace the ratio of tensions is unity, meaning all three wire tensions are equal. As one moves from the center to the outer edges this ratio increases to the maximum ratio of t_max/t_min. As previously noted, this ratio is set by the user and it is clear from the figures that decreasing this ratio decreases the reachable workspace of the robot.

The difference in tensions in the asymmetric robot is less pronounced than it was for rotation; however, there is noticeable warping in the plot. The tension balance point is shifted to the left of the y-axis by several millimeters, and is no longer symmetric. Again, the force constraints limit the reachable workspace.

B. Parallel Force/Position Control

Three different control schemes were tested on the desktop Cerberus robot. Position only and parallel control schemes were tested using the updated statics and inverse kinematics solutions in order to demonstrate the necessity of force control. The parallel control scheme was tested on the naïve geometry as well, where tool rotation is neglected. Each control scheme was tested on both robot geometries.

For each controller and geometry combination the robot was commanded to move the tool head sequentially to a triangular array of points throughout the reachable workspace, as shown in Fig. 7. This testing method closely replicates the operating conditions intended for this robot, in which many injections are done in quick succession.

Fig. 7.

Example of evenly distributed points selected for testing, before filtering based on maximum tension.

Once the controller has reached its desired target, the overhead camera is used to collect ground-truth position measurements. A single trial consists of targeting 44 points for the symmetric geometry and 45 points for the asymmetric geometry. This slight difference is due to the difference in workspace geometry. Each of these trials was repeated ten times to collect statistical data on targeting accuracy and repeatability.

Mean and standard error of the 2D positioning error for the various control schemes for the symmetric and asymmetric geometries are shown in Table III. For all controllers, the error for the symmetric geometry is smaller than for the asymmetric case. For each geometry, mean positioning error decreases when force control is added. In all cases the standard error gets smaller as more force control is added, signifying increase in precision.

TABLE III.

2D Cartesian Error

Mean 2D Error (mm)	Standard Error of Mean (mm)	Control Method	Geometry	IK/Statics Solver
6.79	0.02	Position Only	1:1	New
12.11	0.04		1:1.25
1.81	0.03	Position and Force	1:1	New
1.77	0.02			Old
2.56	0.03		1:1.25	New
2.78	0.02			Old

IV. DISCUSSION

Accounting for the rotation of the tool head does not improve performance significantly for the symmetric robot, it does so for the asymmetric robot. From initial prototypes, it appears that an asymmetrical robot is necessary to properly cover the entire left ventricle. Work to determine the clinically relevant range of arm ratios is ongoing.

From Table III, it is clear that adding force control in increasing amounts improved accuracy and precision. Higher force control gains, however, behaved unreliably.

The difference in performance of all controllers for the different geometries appears to be due to having more points at locations which require higher ratios of tension distribution. Inspection of Fig. 7 in relation to Fig. 6(d) shows that a larger proportion of the target points lies in areas with higher tension disparities than would be the case for the symmetric robot.

Most of the error associated with position control was due to tension buildup, where motors could no longer rotate to change string lengths because the tension was too high. This is due to inaccuracies in modeling the geometry of the system and unmodeled factors such as friction.

For both geometries, the system provided acceptable accuracy and repeatability, approaching 1.8 mm and 2.8 mm for the symmetric and asymmetric cases, respectively, using only relatively low-quality measurements of wire length and tension for feedback. While the required accuracy for gene therapy injections is yet unknown, accuracy of 5 mm is often used as a goal for minimally invasive heart surgery [20].

Further work is required to determine the range of tensions to be used in practice. Current values are estimates which may vary significantly from ideal values. We must also look into the interplay between this acceptable force envelope and the size and shape of the robot workspace.

Extensions to this work, which are ongoing, include further refinements to the manipulator and control hardware, as well as moving the benchtop system from a planar surface to a curved surface which more accurately approximates the surface of the heart. We intend to extend the presented methods first to curved surfaces and finally to periodically deforming curved surfaces.