Modeling, Simulation and Control of the Double Delta Surgical Robot

Abstract

Robotic surgery has been steadily growing, with many new platforms entering the field. Research platforms, however, are limited in number, require a sizable capital expenditure or are difficult to access. This paper presents the analysis and development of a novel surgical manipulator based on parallel kinematics, utilizing the Delta robot as a foundational element. We investigate various aspects including kinematics, statics, workspace and constraints of the manipulator. Additionally, a physics-based model is constructed to validate the analysis and facilitate the creation of a control algorithm aimed at input tracking, particularly for teleoperation purposes. Two experiments are conducted to evaluate the manipulator’s performance: one focusing on circle tracking and a second one employing real kinematic data from a suturing task. The results indicate a maximum tracking error under 1 mm and an RMS error below 0.6 mm for the first trial and 0.3 mm by 2 mm for the suturing tracking task, respectively. Furthermore, through non-linear Bode analysis we demonstrate that the closed-loop system effectively decouples input–output cross-gain terms while maintaining minimal amplification in the diagonal terms. This suggests that the system is well-suited for the intricate and precise motions required in surgical procedures.

1. Introduction

Robotic surgery has been in clinical practice for more than two decades and is steadily expanding, both in the volume of procedures per year and in indications. Even though the market for laparoscopic surgery is dominated by Intuitive Surgical (Sunnyvale, CA, USA) with its da Vinci line of surgical robots, more and more competitors have appeared in the last five years, challenging Intuitive’s prominent position. Competing platforms include the Versius Surgical Robotic System from CMR, the Hinotori Surgical Robot from Medicaroid, Medtronics’s Hugo RAS System, the Revo Surgical Solution from Meerecompany and others. At the same time, research in the field has also been growing [1], covering various fields such as the introduction of haptic feedback [2], the use of augmented reality [3], modeling and evaluation of surgical skill [4,5], autonomous surgery [6], surgical data science [7], telesurgery [8] and so on.

A few research platforms have emerged to facilitate academic research and lower the entry barrier for academic teams. These platforms come in the form of customized one-of-a-kind designs or simplified variations of the existing market models. For instance, starting in 2012, Intuitive Surgical introduced the da Vinci research kit (dVRK), which represents a collaborative effort between academia and industry to re-purpose retired first-generation da Vinci systems for research purposes [9]. Access to the dVRK is selective, and the global community involved in this initiative comprises approximately 40 research centers, with the United States taking a leading role. The underlying code base has been publicly released as open-source. However, several issues have been identified with the platform, namely, reduced precision, inaccurate forward kinematics, link play, inaccurate hand-eye calibration and more [10]. Another notable project is the RAVEN-II robot [11], which has been commercialized by Applied Dexterity Inc as a research platform and is also open-source. Its install base counts about twenty sites worldwide. However, no vision system or master controllers are included, which have to be bought separately. Furthermore, its acquisition cost is well over $150,000, requiring a sizable expenditure to integrate a complete system.

Certain laboratories have opted to develop their own proprietary platforms for research. One such example is the modular and open platform for surgical robotics research (MOPS) robot, created at the University of Southern Denmark [12]. It utilizes off-the-shelf UR5 and UR5e robotic arms from Universal Robots A/S and incorporates custom adapters to attach Da Vinci’s EndoWrist surgical instruments. Another example is the MiroSurge platform by the German Aerospace Center (DLR), which offers a telesurgical solution that features lightweight bed-mounted manipulators [13].

All of the aforementioned platforms employ a serial kinematic chain. This design choice provides a substantial workspace for surgical tools and offers straightforward solutions for forward kinematics. However, it often results in non-unique solutions for the inverse kinematics, whereas the servomotors required to drive the base joints are larger due to the need to support the weight of all the links in the forward chain. Furthermore, precision can be compromised due to link flexure and joint play, necessitating bulkier and more rigid links.

Parallel robots emerge as an appealing alternative due to their simpler inverse kinematic solutions, enhanced rigidity, increased speed and accuracy and the ability to support more load weight relative to their own mass [14]. Their parallel structure also means that errors in the kinematic chain have a reduced impact on the precision of the end effector [15]. However, they often come with a smaller workspace and more complex forward kinematics. Consequently, some researchers have explored the use of parallel robots in microsurgery contexts [16], while others have developed platforms tailored for general laparoscopic surgery [17,18,19].

In this research, we introduce an innovative parallel mechanism designed for use as a manipulator in robotic surgery, which builds on the well-established Delta robot as described in [20]. Our approach involves the parallel connection of two Delta robots with a connecting link that carries the surgical tool, as illustrated in Figure 1. This configuration results in a robot with five degrees of freedom characterized by simple kinematics.

The resultant robot, aptly named the “Double Delta”, exhibits the capability to execute a pivot motion around a specified point, resembling the laparoscopic motion around the trocar point. This particular point is known as the “remote center of motion” (RCM) and can be dynamically adjusted as needed rather than being fixed in place. Additionally, the deployment of Delta robots as the building block of a more complex mechanism allows us to take advantage of the extensive body of analysis and control already available. Further benefits include low cost, lightweight design and off-the-shelf availability.

The current design is the third iteration of the “Double Delta” concept. In the initial setup, the two Delta robots were arranged on the same plane and linked by a straight connection that held the surgical tool. To enhance its maneuverability, the platform was affixed to a tilting base, enabling it to assume appropriate pitch angles. A comprehensive description of this design, including an analysis of its kinematics and workspace, can be found in [21].

In the second iteration, the tilting base was abandoned due to its impracticality and the challenges it posed in terms of dynamic control and gravity compensation. Instead, the pitch angle of the tool holder link was adjusted to create a workspace better suited for robotic surgery, enabling the execution of relevant tasks. This modified configuration was employed in a remote telesurgical demonstration over a 5G network [8]. Although this design proved to be more efficient and was validated in a relevant environment, it came at the cost of a noticeably smaller workspace compared to the initial setup.

In the third and current iteration, the tool is further pitched downward, bringing it to a vertical position. The tool holder link is hinged by a parallelogram mechanism, which is actuated by the two Deltas (Figure 1). This provides a stronger and stiffer linkage system, while the manipulator exhibits a wider, more symmetric, workspace. Its modelling, analysis and control are presented in the following sections.

2. Materials and Methods

To analyze the Double Delta, we can dissociate the two driving Delta robots with the upper linkage bearing the surgical tool. Thus, with reference to Figure 1, let ${\mathbf{d}}_{\mathbf{0}}$, ${\mathbf{d}}_1$ be the tool center points of the first and second Deltas. These exhibit three translational degrees of freedom (DOF) each and are controlled by regulating the angles (${\varphi }_{01}$, ${\varphi }_{02}$, ${\varphi }_{03}$, ${\varphi }_{11}$, ${\varphi }_{12}$, ${\varphi }_{13}$) of their respective motors. Points ${\mathbf{d}}_{\mathbf{0}}$ and ${\mathbf{d}}_1$ are considered known in their respective frames ${\mathbf{D}}_{\mathbf{0}}$ and ${\mathbf{D}}_{\mathbf{1}}$ of their Delta robots, which, in turn, are rigidly tied to the platform’s base frame ${\mathbf{F}}_{\mathbf{B}}$ (see Figure 2 for the definition of the coordinate frames).

The upper linkage mechanism consists of two parallel legs that attach to the tool holder; the first is demarcated by points (${\mathbf{d}}_{\mathbf{0}}$, ${\mathbf{q}}_{\mathbf{0}}$, ${\mathbf{r}}_{\mathbf{0}}$, ${\mathbf{j}}_{\mathbf{0}}$) and the second by points (${\mathbf{d}}_{\mathbf{1}}$, ${\mathbf{q}}_{\mathbf{1}}$, ${\mathbf{r}}_{\mathbf{1}}$, ${\mathbf{j}}_{\mathbf{1}}$). The following list defines the various links and their lengths:

• Link $L_1$ defined by ${\mathbf{d}}_{\mathbf{0}}$,${\mathbf{q}}_{\mathbf{0}}$; length ${\ell }_1$
• Link $L_2$ defined by ${\mathbf{q}}_{\mathbf{0}}$,${\mathbf{j}}_{\mathbf{0}}$; length ${\ell }_2$;
• Link $L_3$ defined by ${\mathbf{d}}_{\mathbf{1}}$,${\mathbf{q}}_{\mathbf{1}}$; length ${\ell }_3$;
• Link $L_4$ defined by ${\mathbf{q}}_{\mathbf{1}}$,${\mathbf{j}}_{\mathbf{1}}$; length ${\ell }_4$
• Link $L_5$ defined by ${\mathbf{r}}_{\mathbf{1}}$,${\mathbf{r}}_{\mathbf{0}}$; length H
• Link $L_H$ defined by ${\mathbf{j}}_{\mathbf{1}}$,${\mathbf{j}}_{\mathbf{0}}$; length H

The links $L_1$ and $L_3$ are rigidly attached to the plates of their respective Delta robots, thus having three translational DoFs. They are aligned with the $Z_{D0}$ and $Z_{D1}$ axes (and, by extension, to the $Z_B$ axis of the base frame). Links $L_2$ and $L_4$ are perpendicular to $L_1$ and $L_3$ and rotate about $Z_2$ and $Z_4$ with an angle $\theta$. They attach to link $L_5$ and the tool holder link $L_H$. These form the parallelogram mechanism (${\mathbf{r}}_{\mathbf{0}}$,${\mathbf{j}}_{\mathbf{0}}$,${\mathbf{j}}_{\mathbf{1}}$,${\mathbf{r}}_{\mathbf{1}})$, which keeps the links $L_2$ and $L_4$ parallel. The length of the horizontal links of the mechanism (${\mathbf{r}}_{\mathbf{0}}$-${\mathbf{j}}_{\mathbf{0}}$,${\mathbf{r}}_{\mathbf{1}}$-${\mathbf{j}}_{\mathbf{1}}$) is S, while for the vertical ones (${\mathbf{r}}_{\mathbf{0}}$-${\mathbf{r}}_{\mathbf{1}}$,${\mathbf{j}}_{\mathbf{0}}$-${\mathbf{j}}_{\mathbf{1}}$) it is H.

The tool holder link is hinged at points ${\mathbf{j}}_{\mathbf{0}}$ and ${\mathbf{j}}_{\mathbf{1}}$ and rotates about the two axes that pass through them. Since $L_H$ is a side of the parallelogram mechanism, we can characterize the rotation of all axes (and orientation of all links) of the mechanism using a single frame. We define such a frame on ${\mathbf{j}}_{\mathbf{1}}$, called the tool holder frame ${\mathbf{F}}_{\mathbf{H}}$, with its $Z_H$ axis being the axis of rotation of $L_H$ through ${\mathbf{j}}_{\mathbf{1}}$.

The tool holder bears a driving mechanism that controls an Endowrist surgical tool using four HerkuleX DRS-0201 servos. At its tip lies the tool point ${\mathbf{P}}_{\mathbf{T}}$ and the tool frame ${\mathbf{F}}_{\mathbf{T}}$, which is rigidly attached to the tool holder. Apparently, frames ${\mathbf{F}}_{\mathbf{H}}$ and ${\mathbf{F}}_{\mathbf{T}}$ are parallel, differing by a fixed translational and rotational component. The transformation between ${\mathbf{F}}_{\mathbf{T}}$ and ${\mathbf{F}}_{\mathbf{H}}$, with reference to Figure 1, is given by

$${}_T{}^H\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{-\pi }{2}}\right)& {}^H{\mathbf{S}}_{\mathbf{T}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right)$$

(1)

Vector ${}^H{\mathbf{S}}_{\mathbf{T}}={[{\ell }_T,D,0]}^T$ expresses the displacement of ${\mathbf{P}}_{\mathbf{T}}$ in the tool holder frame and is constant. Since the two Delta robots are rigidly attached to the platform’s base, the transformation from each Delta’s base frame ${\mathbf{D}}_{\mathbf{0}},{\mathbf{D}}_{\mathbf{1}}$ to the platform’s base frame ${\mathbf{F}}_{\mathbf{B}}$, is constant and defined as

$${}_{D_0}{}^B\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{2\pi }{3}}\right)& {\mathbf{S}}_{\mathbf{B}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right), {}_{D_1}{}^B\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{-\pi }{3}}\right)& -{\mathbf{S}}_{\mathbf{B}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right)$$

(2)

where ${\mathbf{R}}_{\mathbf{z}}$ is the rotation matrix about the Z axis and ${\mathbf{S}}_{\mathbf{B}}={[({\ell }_4-{\ell }_2)/2,0,0]}^T$; hence, the base frame is midways between the two Deltas.

The mapping from the base frame to frames ${\mathbf{F}}_{\mathbf{1}}$ and ${\mathbf{F}}_{\mathbf{3}}$ is straightforward since they have the same orientation but differ in the translational component; that is,

$${}_1{}^B\mathbf{T}=\left(\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {}^B{\mathbf{d}}_{\mathbf{0}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right), {}_3{}^B\mathbf{T}=\left(\begin{array}{cc}{\mathbf{I}}_{3\times 3}& {}^B{\mathbf{d}}_{\mathbf{1}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right)$$

(3)

where

$${}^B{\mathbf{d}}_{\mathbf{0}}={}_{D_0}{}^B\mathbf{T}{}^{D_0}{\mathbf{d}}_{\mathbf{0}}, {}^B{\mathbf{d}}_{\mathbf{1}}={}_{D_1}{}^B\mathbf{T}{}^{D_1}{\mathbf{d}}_{\mathbf{1}}$$

(4)

are the tool center points of the two Deltas in the platform’s base frame. To calculate the mapping from ${\mathbf{F}}_{\mathbf{1}},{\mathbf{F}}_{\mathbf{3}}$ to ${\mathbf{F}}_{\mathbf{2}},{\mathbf{F}}_{\mathbf{4}}$ respectively, we first note that the rotation about axes $Z_2,Z_4$ is the same and denoted as $\theta$. This can be easily inferred by observing that links $L_2,L_4$ are fixed on the tool holder link and rotate along with it. Essentially, the axes $X_2,X_4$ must always be on the same plane as the mechanism and thus must always be parallel. The transformations are hence given by

$${}_2{}^1\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}})& {}^1{\mathbf{S}}_{\mathbf{1}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right), {}_4{}^3\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}})& {}^3{\mathbf{S}}_{\mathbf{3}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right)$$

(5)

Matrix ${\mathbf{R}}_{\mathbf{xyz}}(a,b,c)={\mathbf{R}}_{\mathbf{x}}\left(a\right){\mathbf{R}}_{\mathbf{y}}\left(b\right){\mathbf{R}}_{\mathbf{z}}\left(c\right)$ is the chained rotation about the axes $X,Y$ and Z, while the vectors ${}^1{\mathbf{S}}_{\mathbf{1}}={[0,0,{\ell }_1]}^T$ and ${}^3{\mathbf{S}}_{\mathbf{3}}={[0,0,{\ell }_3]}^T$. For the rotation of the tool holder frame ${\mathbf{T}}_{\mathbf{H}}$ about $Z_H$, we note that its axis of rotation coincides with the joint axes of the parallelogram mechanism, and thus its angle of rotation is $\psi$. The transformations from ${\mathbf{F}}_{\mathbf{H}}$ to ${\mathbf{F}}_{\mathbf{2}}$ and ${\mathbf{F}}_{\mathbf{4}}$ are given by

$${}_H{}^2\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{xzx}}(\psi ,{\displaystyle \frac{-\pi }{2}},{\displaystyle \frac{\pi }{2}})& {}^2{\mathbf{S}}_{\mathbf{2}}-{}^2{\mathbf{S}}_{\mathbf{H}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right), {}_H{}^4\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{xzx}}(\psi ,{\displaystyle \frac{-\pi }{2}},{\displaystyle \frac{\pi }{2}})& {}^4{\mathbf{S}}_{\mathbf{4}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right)$$

(6)

where ${}^2{\mathbf{S}}_{\mathbf{2}}={[0,0,{\ell }_2]}^T$, ${}^4{\mathbf{S}}_{\mathbf{4}}={[0,0,{\ell }_4]}^T$ and ${}^H{\mathbf{S}}_{\mathbf{H}}={[H,0,0]}^T$ are constant displacement vectors in their respective frames. Vector ${\mathbf{S}}_{\mathbf{H}}$ can be expressed in frame ${\mathbf{F}}_{\mathbf{2}}$ as ${}^2{\mathbf{S}}_{\mathbf{H}}={\mathbf{R}}_{\mathbf{xyz}}(\psi ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){}^H{\mathbf{S}}_{\mathbf{H}}$.

In the following, to simplify the notation, all quantities without a left superscript will refer to the base frame ${\mathbf{F}}_{\mathbf{B}}$ unless explicitly given in a different frame, in which case, the appropriate superscript will be noted. The same also holds in case of ambiguities about the frames in which the variables are expressed.

2.1. Forward Kinematics

Formally, forward kinematics refers to computing the position and orientation of the robot’s end effector, i.e., frame $\mathbf{T}$, given the joint angles (${\varphi }_{01}$, ${\varphi }_{02}$, ${\varphi }_{03}$, ${\varphi }_{11}$, ${\varphi }_{12}$, ${\varphi }_{13}$) of the two deltas. However, we consider the points ${\mathbf{d}}_{\mathbf{0}}$, ${\mathbf{d}}_1$ known in ${\mathbf{F}}_{\mathbf{B}}$, obtained by the forward and inverse kinematic solutions of the Deltas, as described in [20]. Thus, the analysis of the platform can focus on the upper linkage mechanism. Using the link transformations described in the previous section, the map from the tool frame to the base frame is given by (see Appendix A for the proof)

$$\begin{array}{c}\hfill {}_T{}^B\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{T}}(\theta ,\psi )& {\mathbf{R}}_{\mathbf{T}}(\theta ,\psi ){\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}+{\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){}^4{\mathbf{S}}_{\mathbf{4}}+{}^B{\mathbf{d}}_{\mathbf{1}}+{}^3{\mathbf{S}}_{\mathbf{3}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right)\end{array}$$

(7)

where

$${\mathbf{R}}_{\mathbf{T}}(\theta ,\psi )={\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){\mathbf{R}}_{\mathbf{xz}}(\psi ,{\displaystyle \frac{-\pi }{2}})$$

(8)

is the orientation matrix of the tool in the base frame. This can be further simplified by using the following identity regarding rotation matrices and an axis of rotation:

Proposition 1.

Given a rotation about axis $\widehat{n}$ by an angle ϕ, denoted by matrix $\mathbf{R}(\varphi ,\widehat{n})$, and an orthogonal matrix $\mathbf{A}$ that transforms the rotation axis, the following relation holds:

$$\mathbf{R}(\varphi ,\widehat{n})={\mathbf{A}}^{⊺}\mathbf{R}(\varphi ,\mathbf{A}\widehat{n})\mathbf{A}$$

(9)

Using this relation, we can change the rotation about the x-axis to the y-axis viz.

$$\begin{array}{cc}\hfill \mathbf{A}& ={\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{\pi }{2}}\right){\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{\pi }{2}}\right)={\mathbf{R}}_{\mathbf{yz}}({\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}})\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill {\mathbf{R}}_{\mathbf{x}}\left(\psi \right)& ={\mathbf{R}}_{\mathbf{zy}}({\displaystyle \frac{-\pi }{2}},{\displaystyle \frac{-\pi }{2}}){\mathbf{R}}_{\mathbf{y}}\left(\psi \right){\mathbf{R}}_{\mathbf{yz}}({\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}})\hfill \end{array}$$

(11)

Plugging into Equation (8), we obtain

$$\begin{array}{cc}\hfill {\mathbf{R}}_{\mathbf{T}}(\theta ,\psi )& ={\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){\mathbf{R}}_{\mathbf{zy}}({\displaystyle \frac{-\pi }{2}},{\displaystyle \frac{-\pi }{2}}){\mathbf{R}}_{\mathbf{y}}\left(\psi \right){\mathbf{R}}_{\mathbf{yz}}({\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{-\pi }{2}}\right)\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill & ={\mathbf{R}}_{\mathbf{x}}\left(\theta \right){\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{\pi }{2}}\right)\left({\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{\pi }{2}}\right){\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{-\pi }{2}}\right)\right){\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{-\pi }{2}}\right){\mathbf{R}}_{\mathbf{y}}\left(\psi \right){\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{\pi }{2}}\right)\left({\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{\pi }{2}}\right){\mathbf{R}}_{\mathbf{z}}\left({\displaystyle \frac{-\pi }{2}}\right)\right)\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill & ={\mathbf{R}}_{\mathbf{x}}\left(\theta \right)\left({\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{\pi }{2}}\right){\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{-\pi }{2}}\right)\right){\mathbf{R}}_{\mathbf{y}}\left(\psi \right){\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{\pi }{2}}\right)\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(14)

and dropping the argument notation, we finally arrive at the following equation:

$${\mathbf{R}}_{\mathbf{T}}={\mathbf{R}}_{\mathbf{x}}\left(\theta \right){\mathbf{R}}_{\mathbf{y}}\left(\psi \right){\mathbf{R}}_{\mathbf{y}}\left({\displaystyle \frac{\pi }{2}}\right)$$

(15)

Analyzing Equation (15), we observe that in the base frame, it consists of a pitch rotation about $Y_B$ and a roll rotation about $X_B$. In the rest position, i.e., $\psi =\theta =0$, the tool frame is pitched downward by $\pi /2$ in ${\mathbf{F}}_{\mathbf{B}}$. The actual orientation angles of the tool in the base frame are given by

$$\begin{array}{cc}\hfill {\psi }_T& =\pi /2+\psi \hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill {\theta }_T& =\theta \hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(17)

Substituting $\psi ,\theta$ in Equation (15) and expanding, the orientation matrix of the tool is found to be

$${\mathbf{R}}_{\mathbf{T}}={\mathbf{R}}_{\mathbf{x}}\left({\theta }_T\right){\mathbf{R}}_{\mathbf{y}}\left({\psi }_T\right)=\left[\begin{array}{ccc}\mathrm{c}{\psi }_T& 0& \mathrm{s}{\psi }_T\\ \mathrm{s}{\theta }_T\mathrm{s}{\psi }_T& \mathrm{c}{\theta }_T& -\mathrm{s}{\theta }_T\mathrm{c}{\psi }_T\\ -\mathrm{c}{\theta }_T\mathrm{s}{\psi }_T& \mathrm{s}{\theta }_T& \mathrm{c}{\theta }_T\mathrm{c}{\psi }_T\end{array}\right]$$

(18)

where, as usual, $\mathrm{c}x=cosx$ and $\mathrm{s}x=sinx$.

In ${\mathbf{F}}_{\mathbf{T}}$ the rotation matrix is decomposed into a pitch rotation about $Y_T$, followed by a yaw rotation about $Z_T$. This means that the surgical tool cannot rotate about its axis, which can be easily understood by noting that links $L_2,L_4$ act on the axis $X_H$ without an arm and, therefore, cannot produce a moment about that axis. To simplify the analysis, we model the platform abstractly using Figure 3.

Using homogeneous coordinates, we express all vectors in the base frame. Slightly abusing notation, the simple calculations show

$$\begin{array}{cc}\hfill {\mathbf{S}}_{\mathbf{1}}=& {}_1{}^B\mathbf{T}{}^1{\mathbf{S}}_{\mathbf{1}}={}^1{\mathbf{S}}_{\mathbf{1}}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill {\mathbf{S}}_{\mathbf{3}}=& {}_3{}^B\mathbf{T}{}^3{\mathbf{S}}_{\mathbf{3}}={}^3{\mathbf{S}}_{\mathbf{3}}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill {\mathbf{S}}_{\mathbf{2}}=& {}_1{}^B\mathbf{T}{}_2{}^1\mathbf{T}{}^2{\mathbf{S}}_{\mathbf{2}}={\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){}^2{\mathbf{S}}_{\mathbf{2}}\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill {\mathbf{S}}_{\mathbf{4}}=& {}_3{}^B\mathbf{T}{}_4{}^3\mathbf{T}{}^4{\mathbf{S}}_{\mathbf{4}}={\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){}^4{\mathbf{S}}_{\mathbf{4}}\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill {\mathbf{S}}_{\mathbf{T}}=& {}_3{}^B\mathbf{T}{}_4{}^3\mathbf{T}{}_H{}^4\mathbf{T}{}^H{\mathbf{S}}_{\mathbf{T}}={\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}\hfill \end{array}$$

(23)

Adding (20), (22) and (23), we obtain

$${\mathbf{S}}_{\mathbf{3}}+{\mathbf{S}}_{\mathbf{4}}+{\mathbf{S}}_{\mathbf{T}}={}^3{\mathbf{S}}_{\mathbf{3}}+{\mathbf{R}}_{\mathbf{xyz}}(\theta ,{\displaystyle \frac{\pi }{2}},{\displaystyle \frac{\pi }{2}}){}^4{\mathbf{S}}_{\mathbf{4}}+{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}$$

(24)

Furthermore, by observing Figure 3, we can traverse the two legs and define the following two vectors:

$$\begin{array}{cc}\hfill {\mathbf{P}}_{\mathbf{0}}& ={\mathbf{d}}_{\mathbf{0}}+{\mathbf{S}}_{\mathbf{1}}+{\mathbf{S}}_{\mathbf{2}}\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{\mathbf{1}}& ={\mathbf{d}}_{\mathbf{1}}+{\mathbf{S}}_{\mathbf{3}}+{\mathbf{S}}_{\mathbf{4}}\hfill \end{array}$$

(26)

Note that vectors ${\mathbf{S}}_{\mathbf{1}},{\mathbf{S}}_{\mathbf{2}},{\mathbf{S}}_{\mathbf{3}},{\mathbf{S}}_{\mathbf{4}}$ are all constants when expressed in the base frame, viz,

$$\begin{array}{cc}\hfill {}^B{\mathbf{S}}_{\mathbf{12}}={}^B{\mathbf{S}}_{\mathbf{1}}+{}^B{\mathbf{S}}_{\mathbf{2}}& =[{\ell }_2,0,{\ell }_1]\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill {}^B{\mathbf{S}}_{\mathbf{34}}={}^B{\mathbf{S}}_{\mathbf{3}}+{}^B{\mathbf{S}}_{\mathbf{4}}& =[{\ell }_4,0,{\ell }_3]\hfill \end{array}$$

(28)

This means that the motion of the points ${\mathbf{j}}_{\mathbf{0}},{\mathbf{j}}_{\mathbf{1}}$ directly mirrors that of ${\mathbf{d}}_{\mathbf{0}},{\mathbf{d}}_{\mathbf{1}}$. Thus kinematically, we can consider that the two Deltas are attached to ${\mathbf{j}}_{\mathbf{0}},{\mathbf{j}}_{\mathbf{1}}$. As is shown in the following, this observation is very helpful for efficiently calculating the inverse kinematics of the platform. Substituting Equations (26) and (24) into Equation (7), the transformation from the base frame to the tip can be written as

$$\begin{array}{c}\hfill {}_T{}^B\mathbf{T}=\left(\begin{array}{cc}{\mathbf{R}}_{\mathbf{T}}& {\mathbf{P}}_{\mathbf{1}}+{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}\\ {\mathbf{0}}_{1\times 3}& 1\end{array}\right)\end{array}$$

(29)

The tool point ${\mathbf{P}}_T$ in the base frame can thus be written as

$${\mathbf{P}}_{\mathbf{T}}={\mathbf{P}}_{\mathbf{1}}+{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}$$

(30)

Note that both the rotation matrix ${\mathbf{R}}_{\mathbf{T}}$ and the tool point ${\mathbf{P}}_T$ depend on the angles ${\psi }_T$ and ${\theta }_t$. Thus, they must be solved with respect to the input variables ${\mathbf{d}}_{\mathbf{0}},{\mathbf{d}}_{\mathbf{1}}$. To this end, by observing Figure 3 we see that the vector ${\mathbf{S}}_{\mathbf{H}}$ is identical to ${\mathbf{P}}_{\mathbf{0}}-{\mathbf{P}}_{\mathbf{1}}$. In its local frame ${\mathbf{F}}_{\mathbf{H}}$, ${}^H{\mathbf{S}}_{\mathbf{H}}$ is constant. However, when expressed in the base frame, it is equal to ${\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{X}}(\pi /2){}^H{\mathbf{S}}_{\mathbf{H}}$. We then have the following equation:

$${\mathbf{P}}_{\mathbf{0}}-{\mathbf{P}}_{\mathbf{1}}={\mathbf{S}}_{\mathbf{H}}={\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{H}}$$

(31)

Expanding the rotation matrices and performing calculations, we obtain

$${\mathbf{P}}_{\mathbf{0}}-{\mathbf{P}}_{\mathbf{1}}=\left[\begin{array}{c}{S}_{H,x}\\ S_{H,y}\\ S_{H,z}\end{array}\right]=H\left[\begin{array}{c}cos{\psi }_T\\ sin{\psi }_{T}sin{\theta }_T\\ -sin{\psi }_{T}cos{\theta }_T\end{array}\right]$$

(32)

and solving for ${\psi }_T,{\theta }_T$,

$$\begin{array}{cc}\hfill {\psi }_T& =arccos(S_{H,x}/H)=\mathrm{atan}2(\sqrt{H^2-S_{H,x}^2},S_{H,x})\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill {\theta }_T& =\mathrm{atan}2(S_{H,y},-S_{H,z})\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(34)

For a pitch angle of ${\psi }_T=0$ or $\pi$ there is a kinematic singularity that leads to an undefined yaw angle ${\theta }_T$ (i.e., ${\theta }_T=\mathrm{atan}2(0,0)$). In this configuration, the links $\ell 2$ and $\ell 4$ coincide, which is mechanically impossible. We have thus constrained the pitch angle to $-{60}^{\circ }+\pi /2\le {\psi }_T\le {60}^{\circ }+\pi /2$.

2.2. Inverse Kinematics

For the inverse kinematics, we consider the tool point ${\mathbf{P}}_{\mathbf{T}}$ and the orientation angles ${\psi }_T,{\theta }_T$, known in the base frame. Then, we must calculate the points ${\mathbf{d}}_{\mathbf{0}},{\mathbf{d}}_{\mathbf{1}}$. Trivially, using Equation (18), we compute the orientation matrix ${\mathbf{R}}_{\mathbf{T}}$, and combining Equations (30) and (26), we solve for ${\mathbf{d}}_{\mathbf{1}}$, viz.

$$\left.\begin{array}{cc}\hfill {\mathbf{P}}_{\mathbf{T}}={\mathbf{P}}_{\mathbf{1}}+{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}& \\ \hfill {\mathbf{P}}_{\mathbf{1}}={\mathbf{d}}_{\mathbf{1}}+{}^B{\mathbf{S}}_{\mathbf{34}}\end{array}\right\}\Rightarrow {\mathbf{d}}_{\mathbf{1}}={\mathbf{P}}_{\mathbf{T}}-{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}-{}^B{\mathbf{S}}_{\mathbf{34}}$$

(35)

Similarly, using Equation (31) and solving for ${}^B{\mathbf{d}}_{\mathbf{0}}$,

$${\mathbf{d}}_{\mathbf{0}}={\mathbf{P}}_{\mathbf{T}}+{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right)({}^H{\mathbf{S}}_{\mathbf{H}}-{}^H{\mathbf{S}}_{\mathbf{T}})-{}^B{\mathbf{S}}_{\mathbf{12}}$$

(36)

The computed center points, denoted as ${\mathbf{d}}_{\mathbf{0}}$ and ${\mathbf{d}}_{\mathbf{1}}$, for the two Deltas may not necessarily indicate valid solutions, as they could lie outside their workspace. Consequently, valid values for ${\mathbf{d}}_{\mathbf{0}},{\mathbf{d}}_{\mathbf{1}}$ are confined within their respective Delta workspaces, necessitating an inclusion test to verify the specific inverse kinematic solution. The Delta robot’s workspace, illustrated in Figure 4A, can be approximated by an inscribed cone with height h and radius r. This simplified geometric approximation streamlines the inclusion test, significantly reducing computational overhead and making it suitable for real-time control.

In pursuit of this objective, consider the workspace frame ${\mathbf{F}}_{{\mathbf{W}}_{\mathbf{i}}}$ corresponding to the Delta robot “i”, as depicted in Figure 4B. Positioned at the cone’s vertex, this frame has its Z-axis directed toward the base. The alignment of frame ${\mathbf{F}}_{{\mathbf{W}}_{\mathbf{i}}}$ with the Delta’s base frame ${\mathbf{D}}_{\mathbf{i}}$ is established through a known transformation denoted ${}_{D_i}{}^{Wi}\mathbf{T}$. Consequently, the transformation of points ${\mathbf{d}}_{\mathbf{i}}$, $i\in \{0,1\}$ is carried out within ${\mathbf{F}}_{{\mathbf{W}}_{\mathbf{i}}}$ as follows:

$${}^{W_i}{\mathbf{d}}_{\mathbf{i}}={}_{D_i}{}^{W_i}\mathbf{T}{}_B{}^{D_i}\mathbf{T}{}^B{\mathbf{d}}_{\mathbf{i}}=(x_i,y_i,z_i)$$

(37)

Now, define the following coordinates in ${\mathbf{F}}_{W_i}$:

$$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]=\left[\begin{array}{c}{z}_{i}t{\theta }_{i}c{\varphi }_i\\ z_{i}t{\theta }_{i}s{\varphi }_i\\ z_i\end{array}\right],\left[\begin{array}{c}{\theta }_i\\ {\varphi }_i\\ z_i\end{array}\right]=\left[\begin{array}{c}\mathrm{atan}2(\sqrt{x_i^2+y_i^2},z_i)\\ \mathrm{atan}2(y_i,x_i)\\ z_i\end{array}\right]$$

(38)

where $t{\theta }_i=tan{\theta }_i$. Upon examination of Figure 4B, it is evident that given the apex angle of the cone is $2\mathrm{atan}(r/h)$, the inclusion constraints for ${}^{W_i}{\mathbf{d}}_{\mathbf{i}}$ in the cone are as follows:

$$\begin{array}{cc}\hfill & 0\le {\theta }_i\le \mathrm{atan}(r/h)\hfill \\ \hfill & 0\le z_i\le h\hfill \end{array}$$

(39)

2.3. Forward Velocity Kinematics

To calculate the forward velocity kinematics, we consider that the points ${\mathbf{d}}_{\mathbf{0}},{\mathbf{d}}_{\mathbf{1}}$ are known in ${\mathbf{F}}_{\mathbf{B}}$ and have respective known velocities ${\mathbf{v}}_{\mathbf{0}},{\mathbf{v}}_{\mathbf{1}}$. Furthermore, the orientation angles ${\psi }_T,{\theta }_T$ of the tool are also given in the base frame, derived from the forward position kinematics. The goal is to compute the linear velocity ${\mathbf{v}}_{\mathbf{T}}$ of the tool point and the angular velocities ${\mathit{\omega }}_{\mathit{T}}$ of the tool frame ${\mathbf{F}}_{\mathbf{T}}$ in the base frame. Starting from the latter, using Equation (18), the orientation matrix ${\mathbf{R}}_{\mathbf{T}}$ can be found. The angular velocity tensor $\mathbf{\Omega }$ can be calculated directly by the known equation:

$$\mathbf{\Omega }={\dot{\mathbf{R}}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{T}}^T={\displaystyle \frac{\partial {\mathbf{R}}_{\mathbf{T}}}{\partial {\theta }_T}}{\mathbf{R}}_{\mathbf{T}}^T{\dot{\theta }}_T+{\displaystyle \frac{\partial {\mathbf{R}}_{\mathbf{T}}}{\partial {\psi }_T}}{\mathbf{R}}_{\mathbf{T}}^T{\dot{\psi }}_T$$

(40)

Performing the calculation (see Appendix C for the proof), we arrive at the following formula:

$$\mathbf{\Omega }=\left[\begin{array}{ccc}0& -{\dot{\psi }}_T\mathrm{s}{\theta }_T& {\dot{\psi }}_T\mathrm{c}{\theta }_T\\ {\dot{\psi }}_T\mathrm{s}{\theta }_T& 0& -{\dot{\theta }}_T\\ -{\dot{\psi }}_T\mathrm{c}{\theta }_T& {\dot{\theta }}_T& 0\end{array}\right]$$

(41)

and collecting terms, we can write the angular velocity vector as

$${\mathit{\omega }}_{\mathit{T}}=\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_z\end{array}\right]=\left[\begin{array}{c}{\dot{\theta }}_T\\ {\dot{\psi }}_{T}cos{\theta }_T\\ {\dot{\psi }}_{T}sin{\theta }_T\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& cos{\theta }_T\\ 0& sin{\theta }_T\end{array}\right]\left[\begin{array}{c}{\dot{\theta }}_T\\ {\dot{\psi }}_T\end{array}\right]$$

(42)

For the passive joint angle velocities, differentiating Equations (32) and (33), we obtain (see Appendix B)

$$\left[\begin{array}{c}{\dot{\theta }}_T\\ {\dot{\psi }}_T\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{\Delta v_{z}sin{\theta }_T+\Delta v_{y}cos{\theta }_T}{Hsin{\psi }_T}}\\ {\displaystyle \frac{-\Delta v_x}{Hsin{\psi }_T}}\end{array}\right]={\displaystyle \frac{1}{Hsin{\psi }_T}}\left[\begin{array}{ccc}0& cos{\theta }_T& sin{\theta }_T\\ -1& 0& 0\end{array}\right]\left[\begin{array}{c}\Delta v_x\\ \Delta v_y\\ \Delta v_z\end{array}\right]$$

(43)

where ${\mathbf{v}}_{\mathbf{0}}-{\mathbf{v}}_{\mathbf{1}}=\Delta \mathbf{v}={[\Delta v_x,\Delta v_y,\Delta v_z]}^T$. Combining (43) and (42), the angular velocity can finally be written as

$${\mathit{\omega }}_{\mathit{T}}={\displaystyle \frac{1}{Hsin{\psi }_T}}\left[\begin{array}{ccc}0& \mathrm{c}{\theta }_T& \mathrm{s}{\theta }_T\\ -\mathrm{c}{\theta }_T& 0& 0\\ -\mathrm{s}{\theta }_T& 0& 0\end{array}\right]\Delta \mathbf{v}=\mathbf{K}\Delta \mathbf{v}$$

(44)

Note that the rank of $\mathbf{K}$ is 2, thus ${\omega }_T$ has two independent degrees of freedom. This is due to the fact that the roll of the robot, in the tool frame, is always zero and, hence, has two rotational DoFs. Consequently, the angular velocity can be described by only two components in a proper frame. To calculate the linear velocity of the tool point, we differentiate (30) with respect to time, viz.

$$\begin{array}{c}\hfill {\mathbf{v}}_{\mathbf{T}}={\mathbf{v}}_{\mathbf{1}}+{\dot{\mathbf{R}}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}={\mathbf{v}}_{\mathbf{1}}+\mathbf{\Omega }{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}={\mathbf{v}}_{\mathbf{1}}+\mathbf{\Omega }{\mathbf{S}}_{\mathbf{T}}\end{array}$$

(45)

Expanding the second term on the right-hand side, we obtain (see Appendix D for the proof)

$${\mathbf{v}}_{\mathbf{T}}={\mathbf{v}}_{\mathbf{1}}+{\displaystyle \frac{1}{Hsin{\psi }_T}}\left[\begin{array}{ccc}A& 0& 0\\ -\mathrm{s}{\theta }_{T}B& -{\mathrm{c}}^2{\theta }_{T}A& -{\displaystyle \frac{\mathrm{s}\left(2{\theta }_T\right)}{2}}A\\ \mathrm{c}{\theta }_{T}B& -{\displaystyle \frac{\mathrm{s}\left(2{\theta }_T\right)}{2}}A& -{\mathrm{s}}^2{\theta }_{T}A\end{array}\right]\Delta \mathbf{v}={\mathbf{v}}_{\mathbf{1}}+\mathbf{\Lambda }\Delta \mathbf{v}$$

(46)

where

$$\begin{array}{cc}\hfill A& =\sqrt{D^2+{\ell }_T^2}cos({\psi }_T+\varphi ),B=\sqrt{D^2+{\ell }_T^2}sin({\psi }_T+\varphi )\hfill \\ \hfill \varphi & =arctan({\ell }_T/D)\hfill \end{array}$$

(47)

Combining (44) and (46), we derive the following decoupled velocity equations:

$$\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{T}}\\ {\mathit{\omega }}_{\mathit{T}}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& \mathbf{\Lambda }\\ \mathbf{0}& \mathbf{K}\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{1}}\\ \Delta \mathbf{v}\end{array}\right]$$

(48)

We observe that the angular velocity is decoupled from the first input ${\mathbf{v}}_{\mathbf{1}}$ and depends solely on the relative velocity of the two Deltas. If we denote the actuated joint vector as ${\mathit{\theta }}_a={[{\mathbf{d}}_{\mathbf{1}},{\mathbf{d}}_{\mathbf{0}}]}^T$, the generalized velocity is ${\dot{\mathit{\theta }}}_a={[{\mathbf{v}}_{\mathbf{1}},{\mathbf{v}}_{\mathbf{0}}]}^T$ and the end-effector velocity is $\mathbf{w}={[{\mathbf{v}}_{\mathbf{T}},{\mathit{\omega }}_{\mathit{T}}]}^T$. We can derive the geometric Jacobian from (48) as follows:

$$\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{T}}\\ {\mathit{\omega }}_{\mathit{T}}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}-\mathbf{\Lambda }& \mathbf{\Lambda }\\ -\mathbf{K}& \mathbf{K}\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{1}}\\ {\mathbf{v}}_{\mathbf{0}}\end{array}\right]\Rightarrow \mathbf{w}={\mathbf{J}}_{\mathbf{ee}}{\dot{\mathit{\theta }}}_a$$

(49)

The matrix ${\mathbf{J}}_{\mathbf{ee}}$ is called the direct kinematic Jacobian and is evidently singular, which is expected since the robot exhibits five DoFs, while the input is 6D.

2.4. Inverse Velocity Kinematics

For the inverse velocity kinematics, we consider that the linear and angular velocities $\mathbf{\Omega }$ and ${\mathbf{v}}_{\mathbf{T}}$ of the tool are given, and we seek to calculate the velocities of the actuating joints, i.e., ${\mathbf{v}}_0,{\mathbf{v}}_1$. Again, the positional configuration of the robot is completely known. Consequently, solving for ${\mathbf{v}}_1$ from (45), we have

$$\begin{array}{c}\hfill {\mathbf{v}}_1={\mathbf{v}}_{\mathbf{T}}-\mathbf{\Omega }{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{T}}={\mathbf{v}}_{\mathbf{T}}-\mathbf{\Omega }{\mathbf{S}}_{\mathbf{T}}\end{array}$$

(50)

and combining (58) and (50), ${\mathbf{v}}_0$ is

$${\mathbf{v}}_{\mathbf{0}}={\mathbf{v}}_{\mathbf{T}}+\mathbf{\Omega }{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right)\left({}^H{\mathbf{S}}_{\mathbf{H}}-{}^H{\mathbf{S}}_{\mathbf{T}}\right)={\mathbf{v}}_{\mathbf{T}}+\mathbf{\Omega }\left({\mathbf{S}}_{\mathbf{H}}-{\mathbf{S}}_{\mathbf{T}}\right)$$

(51)

To calculate the inverse kinematic Jacobian, we note that (50) and (51) can be written in vector form as

$$\begin{array}{cc}\hfill {\mathbf{v}}_{\mathbf{1}}& ={\mathbf{v}}_{\mathbf{T}}-{\mathit{\omega }}_{\mathit{T}}\times {\mathbf{S}}_{\mathbf{T}}={\mathbf{v}}_{\mathbf{T}}+{\mathbf{S}}_{\mathbf{T}}\times {\mathit{\omega }}_{\mathit{T}}={\mathbf{v}}_{\mathbf{T}}+{\left[{\mathbf{S}}_{\mathbf{T}}\right]}_{\times }{\mathit{\omega }}_{\mathit{T}}\hfill \\ \hfill {\mathbf{v}}_{\mathbf{0}}& ={\mathbf{v}}_{\mathbf{T}}+{\mathit{\omega }}_{\mathit{T}}\times \left({\mathbf{S}}_{\mathbf{H}}-{\mathbf{S}}_{\mathbf{T}}\right)={\mathbf{v}}_{\mathbf{T}}+\left({\mathbf{S}}_{\mathbf{T}}-{\mathbf{S}}_{\mathbf{H}}\right)\times {\mathit{\omega }}_{\mathit{T}}={\mathbf{v}}_{\mathbf{T}}+\left({\left[{\mathbf{S}}_{\mathbf{T}}\right]}_{\times }-{\left[{\mathbf{S}}_{\mathbf{H}}\right]}_{\times }\right){\mathit{\omega }}_{\mathit{T}}\hfill \end{array}$$

(52)

where ${\left[\mathbf{A}\right]}_{\times }$ is a skew-symmetric matrix of A associated with the cross product. Factoring (52), we come up with the following:

$$\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{1}}\\ {\mathbf{v}}_{\mathbf{0}}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& {\left[{\mathbf{S}}_{\mathbf{T}}\right]}_{\times }\\ \mathbf{I}& {\left[{\mathbf{S}}_{\mathbf{T}}\right]}_{\times }-{\left[{\mathbf{S}}_{\mathbf{H}}\right]}_{\times }\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{T}}\\ {\omega }_{\mathbf{T}}\end{array}\right]\Rightarrow {\dot{\mathit{\theta }}}_a={\mathbf{J}}_{\mathbf{ee}}^{-\mathbf{1}}\mathbf{w}$$

(53)

Equivalently, the inverse velocity equations are

$$\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{1}}\\ \Delta \mathbf{v}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& {\left[{\mathbf{S}}_{\mathbf{T}}\right]}_{\times }\\ \mathbf{0}& -{\left[{\mathbf{S}}_{\mathbf{H}}\right]}_{\times }\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{T}}\\ {\mathit{\omega }}_{\mathit{T}}\end{array}\right]$$

(54)

2.5. Constraints and Forces

Since the two legs act on the same link ${\mathbf{S}}_{\mathbf{H}}$, by traversing them we can denote the following loop closure equation:

$$\begin{array}{cc}\hfill & {\mathbf{d}}_{\mathbf{0}}+{\mathbf{S}}_{\mathbf{1}}+{\mathbf{S}}_{\mathbf{2}}-{\mathbf{S}}_{\mathbf{H}}-{\mathbf{S}}_{\mathbf{4}}-{\mathbf{S}}_{\mathbf{3}}-{\mathbf{d}}_{\mathbf{1}}=0\Rightarrow ({\mathbf{d}}_{\mathbf{0}}+{\mathbf{S}}_{\mathbf{12}})-({\mathbf{d}}_{\mathbf{1}}+{\mathbf{S}}_{\mathbf{34}})={\mathbf{S}}_{\mathbf{H}}\Rightarrow \hfill \\ \hfill & {\mathbf{P}}_{\mathbf{0}}-{\mathbf{P}}_{\mathbf{1}}={\mathbf{S}}_{\mathbf{H}}\hfill \end{array}$$

(55)

It can be readily observed that (55) expresses a geometric constraint of the system that involves the two Delta tool points ${\mathbf{d}}_{\mathbf{0}},{\mathbf{d}}_{\mathbf{1}}$, or equivalently, ${\mathbf{P}}_{\mathbf{0}},{\mathbf{P}}_{\mathbf{1}}$. Under the assumption that the links are totally rigid, by squaring (55), we obtain

$${({\mathbf{P}}_{\mathbf{0}}-{\mathbf{P}}_{\mathbf{1}})}^T({\mathbf{P}}_{\mathbf{0}}-{\mathbf{P}}_{\mathbf{1}})={\mathbf{S}}_{\mathbf{H}}{}^T{\mathbf{S}}_{\mathbf{H}}=H^2$$

(56)

hence, their distance is always constant and equal to the link length. Equivalently, we can use the second equation of (55) and arrive at the following formula:

$$\begin{array}{cc}\hfill & ({\mathbf{d}}_{\mathbf{0}}-{\mathbf{d}}_{\mathbf{1}})-({\mathbf{S}}_{\mathbf{34}}-{\mathbf{S}}_{\mathbf{12}})={\mathbf{S}}_{\mathbf{H}}\Rightarrow \Delta \mathbf{d}-\Delta \mathbf{S}={\mathbf{S}}_{\mathbf{H}}\Rightarrow {\parallel \Delta \mathbf{d}-\Delta \mathbf{S}\parallel }^2=H^2\hfill \end{array}$$

(57)

Equation (57) indicates that $\Delta \mathbf{d}$ is situated on a sphere with a radius of H and centered at $\Delta \mathbf{S}={\mathbf{S}}_{\mathbf{34}}-{\mathbf{S}}_{\mathbf{12}}=const$. Consequently, all solutions to the kinematics equations must adhere to this restriction. This introduces an additional layer of complexity in the context of forward kinematics. To address the problem, we initially assumed that ${\mathbf{d}}_{\mathbf{0}}$ and ${\mathbf{d}}_{\mathbf{1}}$ are given. However, it becomes apparent that this assumption is insufficient; generally, a pair of ${\mathbf{d}}_{\mathbf{0}}$ and ${\mathbf{d}}_{\mathbf{1}}$ does not guarantee valid kinematic configurations for the platform. These values must also satisfy the geometric constraint described in Equation (57).

By differentiating (57), we can arrive at a kinematic constraint involving the two velocities ${\mathbf{v}}_{\mathbf{0}},{\mathbf{v}}_1$, viz.

$$\Delta \mathbf{v}={\mathbf{v}}_{\mathbf{0}}-{\mathbf{v}}_{\mathbf{1}}=\mathbf{\Omega }{\mathbf{R}}_{\mathbf{T}}{\mathbf{R}}_{\mathbf{x}}\left({\displaystyle \frac{\pi }{2}}\right){}^H{\mathbf{S}}_{\mathbf{H}}=\mathbf{\Omega }{\mathbf{S}}_{\mathbf{H}}$$

(58)

Multiplying from the left with ${\mathbf{S}}_{\mathbf{H}}$ and considering that the quadratic form of $\mathbf{\Omega }$ is always zero since it is skew-symmetric, we have

$$\begin{array}{cc}\hfill {{\mathbf{S}}_{\mathbf{H}}}^T\Delta \mathbf{v}& ={{\mathbf{S}}_{\mathbf{H}}}^T\mathbf{\Omega }{\mathbf{S}}_{\mathbf{H}}=0\hfill \end{array}$$

(59)

and expanding

$$\begin{array}{cc}\hfill {{\mathbf{S}}_{\mathbf{H}}}^T{\mathbf{v}}_{\mathbf{0}}& ={{\mathbf{S}}_{\mathbf{H}}}^T{\mathbf{v}}_{\mathbf{1}}\hfill \end{array}$$

(60)

The equation implies that the projections of the velocities of the two joints onto the tool link should be equal. Equivalently, the relative linear velocity $\Delta \mathbf{v}$ of the two actuating joints should be perpendicular to the tool link $L_H$. This means that $\Delta \mathbf{v}$ should always reside in the orthogonal complement of ${\mathbf{S}}_{\mathbf{H}}$, i.e., its orthogonal plane. We observe that ${\mathbf{S}}_{\mathbf{H}}$ is parallel to the x-axis of the tool frame ${\mathbf{F}}_{\mathbf{T}}$. Since its orientation is the matrix ${\mathbf{R}}_{\mathbf{T}}$ in (18), the vectors spanning this plane are the second and third columns of the matrix. Thus, $\Delta \mathbf{v}$ should be of the form

$$\Delta \mathbf{v}=\left[\begin{array}{cc}0& \mathrm{s}{\psi }_T\\ \mathrm{c}{\theta }_T& -\mathrm{s}{\theta }_T\mathrm{c}{\psi }_T\\ \mathrm{s}{\theta }_T& \mathrm{c}{\theta }_T\mathrm{c}{\psi }_T\end{array}\right]\left[\begin{array}{c}\Delta v_a\\ \Delta v_b\end{array}\right]$$

(61)

where ${[\Delta v_a,\Delta v_b]}^T=\Delta {\mathbf{v}}_{\mathbf{c}}\in {\mathbb{R}}^2$ is the constrained relative velocity vector. Substituting $\Delta \mathbf{v}$ in (44) and (45), the new constrained $\mathbf{K},\mathbf{\Lambda }$ matrices are

$${\mathbf{K}}_{\mathbf{c}}={\displaystyle \frac{1}{H\mathrm{s}{\psi }_T}}\left[\begin{array}{cc}1& 0\\ 0& -\mathrm{c}{\theta }_T\mathrm{s}{\psi }_T\\ 0& -\mathrm{s}{\theta }_T\mathrm{s}{\psi }_T\end{array}\right],{\mathbf{\Lambda }}_{\mathbf{c}}={\displaystyle \frac{1}{H\mathrm{s}{\psi }_T}}\left[\begin{array}{cc}0& -A\mathrm{s}{\psi }_T\\ -A\mathrm{c}{\theta }_T& -\mathrm{s}{\theta }_T\mathrm{s}{\psi }_{T}B\\ -A\mathrm{s}{\theta }_T& \mathrm{c}{\theta }_T\mathrm{s}{\psi }_{T}B\end{array}\right]$$

(62)

and the constrained forward decoupled velocity equations are

$$\mathbf{w}=\left[\begin{array}{cc}\mathbf{I}& {\mathbf{\Lambda }}_{\mathbf{c}}\\ \mathbf{0}& {\mathbf{K}}_{\mathbf{c}}\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_{\mathbf{1}}\\ \Delta {\mathbf{v}}_{\mathbf{c}}\end{array}\right]$$

(63)

We note that the (constrained) velocity vector is ${[{\mathbf{v}}_{\mathbf{1}},\Delta {\mathbf{v}}_{\mathbf{c}}]}^T\in {\mathbb{R}}^5$.

Using the inverse kinematic Jacobian, we can write the familiar force/torque transmission equation for the robot, which holds true both for serial and parallel kinematics, viz.

$${\mathbf{F}}_{\mathbf{ee}}={\mathbf{J}}_{\mathbf{ee}}^{-\mathbf{T}}{\mathbf{F}}_{\mathbf{D}}$$

(64)

where ${\mathbf{F}}_{\mathbf{ee}}=[{\mathbf{f}}_{\mathbf{ee}},{\mathbf{m}}_{\mathbf{ee}}]$ is the wrench transmitted to the end effector through the forces ${\mathbf{F}}_{\mathbf{D}}={[{\mathbf{F}}_{\mathbf{1}},{\mathbf{F}}_{\mathbf{0}}]}^T$ applied by the two Deltas on ${\mathbf{d}}_{\mathbf{1}},{\mathbf{d}}_{\mathbf{0}}$. Although the mapping from the Delta forces to the end effector is unique, the singularity of ${\mathbf{J}}_{\mathbf{ee}}^{-\mathbf{T}}$ implies that the inverse is not true; there is an infinite range of solutions that map the end effector wrench onto the two Delta forces. These contain components that lie in the null space $N\left({\mathbf{J}}_{\mathbf{ee}}^{-\mathbf{T}}\right)$ and do not contribute to the motion of the platform; they represent internal antagonistic forces. These can be investigated by setting (64) to null and solving for ${\mathbf{F}}_{\mathbf{D}}$, viz.

$$\mathbf{0}=\left[\begin{array}{cc}\mathbf{I}& \mathbf{I}\\ [{\mathbf{S}}_{\mathbf{T}}{]}_{\times }& {\left[{\mathbf{S}}_{\mathbf{T}}\right]}_{\times }-{\left[{\mathbf{S}}_{\mathbf{H}}\right]}_{\times }\end{array}\right]\left[\begin{array}{c}{\mathbf{F}}_{\mathbf{1}}\\ {\mathbf{F}}_{\mathbf{0}}\end{array}\right]=\left[\begin{array}{c}{\mathbf{F}}_{\mathbf{1}}+{\mathbf{F}}_{\mathbf{0}}\\ {\mathbf{S}}_{\mathbf{T}}{]}_{\times }{\mathbf{F}}_{\mathbf{1}}+{\left({\mathbf{S}}_{\mathbf{T}}\right]}_{\times }-{\left[{\mathbf{S}}_{\mathbf{H}}\right]}_{\times }){\mathbf{F}}_{\mathbf{0}}\end{array}\right]$$

(65)

This leads to the following conditions:

$$\begin{array}{cc}\hfill {\mathbf{F}}_{\mathbf{1}}+{\mathbf{F}}_{\mathbf{0}}& =0\hfill \\ \hfill [{\mathbf{S}}_{\mathbf{H}}{]}_{\times }{\mathbf{F}}_{\mathbf{0}}& =0\hfill \end{array}$$

(66)

Thus, the forces in the null space must be opposite, sum to zero and must lie in the space spanned by ${\mathbf{S}}_{\mathbf{H}}$. Obviously, the null space is $N\left({\mathbf{J}}_{\mathbf{ee}}^{-\mathbf{T}}\right)\equiv t{[-{\mathbf{S}}_{\mathbf{H}},{\mathbf{S}}_{\mathbf{H}}]}^T,t\in \mathbb{R}$.

2.6. Gravity Compensation

In order to implement a control algorithm for the platform, an initial step involves offsetting the influence of gravity. This results in the robot being perceived by the controller as “weightless”. For this purpose, we assume that the robot moves at a sufficiently slow pace, minimizing any inertia effects, which are consequently disregarded. The algorithm for gravity compensation entails determining the necessary forces to be exerted by each Delta robot, ensuring that the platform attains static equilibrium at every pose. To this end, we employ the direct Lagrangian Method to compute the forces [22]. According to this, when the robot is in static equilibrium, the kinetic energy is null and the Lagrangian involves only the contribution of the potential energy U of the system. Then, the equations of motion, using the Euler–Lagrange formulation, transform to

$$\begin{array}{c}\hfill {\displaystyle \frac{\partial \mathrm{U}}{\partial {\lambda }_i}}={\xi }_i\end{array}$$

(67)

where ${\lambda }_i$ is the generalized coordinates of the system and ${\xi }_i$ the generalized forces. In the specific case of our redundantly actuated parallel robot, we compute only the derivatives with respect to the actuated joints. Consequently, the compensation forces are given by

$$\begin{array}{cc}\hfill {\mathbf{F}}_{\mathbf{g}\mathbf{0}}& ={\left[{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0x}}},{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0y}}},{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0z}}}\right]}^T\hfill \\ \hfill {\mathbf{F}}_{\mathbf{g}\mathbf{1}}& ={\left[{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1x}}},{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1y}}},{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1z}}}\right]}^T\hfill \end{array}$$

(68)

To calculate the potential energy of the system, we consider each link separately. The center of mass $m_i$ of link i, where i = 1, 2, 3, 4, 5, H is given in the respective frame ${\mathbf{F}}_{\mathbf{i}}$ by the displacement vector ${}^i\mathbf{r}{\mathbf{c}}_{\mathbf{i}}$ (Figure 5). Casting them into the base frame ${\mathbf{F}}_{\mathbf{b}}$, the potential energies are given by

$$\begin{array}{c}\hfill {\mathrm{U}}_i=m_i\mathbf{g}{}_i{}^B\mathbf{T}{}^i\mathbf{r}{\mathbf{c}}_{\mathbf{i}}\end{array}$$

(69)

with $\mathbf{g}=[0,0,-g]$ being the gravity vector. Note that the frame in link 5 is parallel to the tool holder frame but shifted to ${\mathbf{r}}_{\mathbf{1}}$. By performing the calculations, we arrive at the following formulas:

$$\begin{array}{c}\begin{array}{cccc}\hfill & {\mathrm{U}}_1=-g{m}_1(d_{0z}+r{c}_{13}),\hfill & \hfill & {\mathrm{U}}_3=-g{m}_3(d_{1z}+r{c}_{33})\hfill \\ \hfill & {\mathrm{U}}_2=-g{m}_3(d_{0z}+{\ell }_1)-g{m}_2{\mathbf{rc}}_{\mathbf{2}}·\mathbf{N},\hfill & \hfill & {\mathrm{U}}_4=-g{m}_4(d_{1z}+{\ell }_3)-g{m}_4{\mathbf{rc}}_{\mathbf{4}}·\mathbf{N}\hfill \\ \hfill & {\mathrm{U}}_5=-g{m}_5(d_{1z}+{\ell }_3)+g{m}_5{\mathbf{rc}}_{\mathbf{5}}·\mathbf{M},\hfill & \hfill & {\mathrm{U}}_H=-g{m}_H(d_{1z}+{\ell }_3)+g{m}_H{\mathbf{rc}}_{\mathbf{H}}·\mathbf{M}\hfill \end{array}\\ \mathrm{U}={\mathrm{U}}_1+{\mathrm{U}}_2+{\mathrm{U}}_3+{\mathrm{U}}_4+{\mathrm{U}}_5+{\mathrm{U}}_H\end{array}$$

(70)

where U is the potential energy of the robot, $\mathbf{N}={[s\theta ,c\theta ,0]}^T$ and $\mathbf{M}={[c\psi c\theta ,s\psi c\theta ,s\theta ]}^T$. Taking the partial derivative of U with respect to to each actuated joint, we have

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1x}}}=-g(m_2{\mathbf{rc}}_{\mathbf{2}}+m_4{\mathbf{rc}}_{\mathbf{4}}){\displaystyle \frac{\partial \mathbf{N}}{\partial d_{1x}}}+g(m_5{\mathbf{rc}}_{\mathbf{5}}+m_H{\mathbf{rc}}_{\mathbf{H}}){\displaystyle \frac{\partial \mathbf{M}}{\partial d_{1x}}}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(71)

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1y}}}=-g(m_2{\mathbf{rc}}_{\mathbf{2}}+m_4{\mathbf{rc}}_{\mathbf{4}}){\displaystyle \frac{\partial \mathbf{N}}{\partial d_{1y}}}+g(m_5{\mathbf{rc}}_{\mathbf{5}}+m_H{\mathbf{rc}}_{\mathbf{H}}){\displaystyle \frac{\partial \mathbf{M}}{\partial d_{1y}}}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(72)

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1z}}}=-g(m_2{\mathbf{rc}}_{\mathbf{2}}+m_4{\mathbf{rc}}_{\mathbf{4}}){\displaystyle \frac{\partial \mathbf{N}}{\partial d_{1z}}}+g(m_5{\mathbf{rc}}_{\mathbf{5}}+m_H{\mathbf{rc}}_{\mathbf{H}}){\displaystyle \frac{\partial \mathbf{M}}{\partial d_{1z}}}-g(m_3+m_4+m_5+m_H)\hfill \end{array}$$

(73)

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0x}}}=-g(m_2{\mathbf{rc}}_{\mathbf{2}}+m_4{\mathbf{rc}}_{\mathbf{4}}){\displaystyle \frac{\partial \mathbf{N}}{\partial d_{0x}}}+g(m_5{\mathbf{rc}}_{\mathbf{5}}+m_H{\mathbf{rc}}_{\mathbf{H}}){\displaystyle \frac{\partial \mathbf{M}}{\partial d_{0x}}}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(74)

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0y}}}=-g(m_2{\mathbf{rc}}_{\mathbf{2}}+m_4{\mathbf{rc}}_{\mathbf{4}}){\displaystyle \frac{\partial \mathbf{N}}{\partial d_{0y}}}+g(m_5{\mathbf{rc}}_{\mathbf{5}}+m_H{\mathbf{rc}}_{\mathbf{H}}){\displaystyle \frac{\partial \mathbf{M}}{\partial d_{0y}}}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(75)

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0z}}}=-g(m_2{\mathbf{rc}}_{\mathbf{2}}+m_4{\mathbf{rc}}_{\mathbf{4}}){\displaystyle \frac{\partial \mathbf{N}}{\partial d_{0z}}}+g(m_5{\mathbf{rc}}_{\mathbf{5}}+m_H{\mathbf{rc}}_{\mathbf{H}}){\displaystyle \frac{\partial \mathbf{M}}{\partial d_{0z}}}-g(m_1+m_2)\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hfill \end{array}$$

(76)

Now, let

$$\begin{array}{cccc}\hfill & \mathbf{A}=-g(m_2{\mathbf{rc}}_{\mathbf{2}}+m_4{\mathbf{rc}}_{\mathbf{4}})\hfill & \hfill ,& \mathbf{B}=g(m_5{\mathbf{rc}}_{\mathbf{5}}+m_H{\mathbf{rc}}_{\mathbf{H}})\hfill \\ \hfill & C=-g(m_3+m_4+m_5+m_H)\hfill & \hfill ,& D=-g(m_1+m_2)\hfill \end{array}$$

(77)

We also note that

$$\begin{array}{cc}\hfill {\displaystyle \frac{\partial \mathbf{N}}{\partial d_i}}& ={\displaystyle \frac{\partial \mathbf{N}}{\partial \theta }}{\displaystyle \frac{\partial \theta }{\partial d_i}}+{\displaystyle \frac{\partial \mathbf{N}}{\partial \psi }}{\displaystyle \frac{\partial \psi }{\partial d_i}}=\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{N}}{\partial \theta }}& {\displaystyle \frac{\partial \mathbf{N}}{\partial \psi }}\end{array}\right]\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_i}}\\ {\displaystyle \frac{\partial \psi }{\partial d_i}}\end{array}\right]\hfill \\ \hfill {\displaystyle \frac{\partial \mathbf{M}}{\partial d_i}}& ={\displaystyle \frac{\partial \mathbf{M}}{\partial \theta }}{\displaystyle \frac{\partial \theta }{\partial d_i}}+{\displaystyle \frac{\partial \mathbf{M}}{\partial \psi }}{\displaystyle \frac{\partial \psi }{\partial d_i}}=\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{M}}{\partial \theta }}& {\displaystyle \frac{\partial \mathbf{M}}{\partial \psi }}\end{array}\right]\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_i}}\\ {\displaystyle \frac{\partial \psi }{\partial d_i}}\end{array}\right]\hfill \end{array}$$

(78)

for $i=(0x,0y,0z,1x,1y,1z)$. Combining (78), (77) and (71), we arrive at the following:

$$\begin{array}{cccc}\hfill {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1x}}}& ={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_{1x}}}\\ {\displaystyle \frac{\partial \psi }{\partial d_{1x}}}\end{array}\right]\hfill & \hfill ,{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0x}}}& ={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_{0x}}}\\ {\displaystyle \frac{\partial \psi }{\partial d_{0x}}}\end{array}\right]\hfill \\ \hfill {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1y}}}& ={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_{1y}}}\\ {\displaystyle \frac{\partial \psi }{\partial d_{1y}}}\end{array}\right]\hfill & \hfill ,{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0y}}}& ={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_{0y}}}\\ {\displaystyle \frac{\partial \psi }{\partial d_{0y}}}\end{array}\right]\hfill \\ \hfill {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1z}}}& ={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_{1z}}}\\ {\displaystyle \frac{\partial \psi }{\partial d_{1z}}}\end{array}\right]+C\hfill & \hfill ,{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0z}}}& ={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_{0z}}}\\ {\displaystyle \frac{\partial \psi }{\partial d_{0z}}}\end{array}\right]+D\hfill \end{array}$$

(79)

where

$$\begin{array}{c}\hfill {\mathbf{\Xi }}^T={\mathbf{A}}^T\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{N}}{\partial \theta }}& {\displaystyle \frac{\partial \mathbf{N}}{\partial \psi }}\end{array}\right]+{\mathbf{B}}^T\left[\begin{array}{cc}{\displaystyle \frac{\partial \mathbf{M}}{\partial \theta }}& {\displaystyle \frac{\partial \mathbf{M}}{\partial \psi }}\end{array}\right]\end{array}$$

(80)

Expanding the partial derivatives of $\mathbf{N}$ and $\mathbf{M}$, ${\mathbf{\Xi }}^T$ is found to be

$$\begin{array}{c}\hfill {\mathbf{\Xi }}^T={\mathbf{A}}^T\left[\begin{array}{cc}c\theta & 0\\ -s\theta & 0\\ 0& 0\end{array}\right]+{\mathbf{B}}^T\left[\begin{array}{cc}-c\psi s\theta & -s\psi c\theta \\ -s\psi s\theta & c\psi c\theta \\ c\theta & 0\end{array}\right]\end{array}$$

(81)

For the derivatives of $\psi ,\theta$ with respect to $d_i$, using a similar reasoning as in Equations (A8) and (A11), we can write the following:

$$\begin{array}{c}\hfill {\displaystyle \frac{\partial \psi }{\partial d_i}}={\displaystyle \frac{-\frac{\partial S_{H,x}}{\partial d_i}}{Hc\psi }},{\displaystyle \frac{\partial \theta }{\partial d_i}}={\displaystyle \frac{s\theta \frac{\partial S_{H,z}}{\partial d_i}+c\theta \frac{\partial S_{H,y}}{\partial d_i}}{Hc\psi }}\end{array}$$

(82)

Substituting $S_H$ from (57) and performing calculations,

$$\begin{array}{c}{\displaystyle \frac{\partial \psi }{\partial d_{1x}}}={\displaystyle \frac{1}{Hc\psi }},{\displaystyle \frac{\partial \psi }{\partial d_{0x}}}={\displaystyle \frac{-1}{Hc\psi }}\\ {\displaystyle \frac{\partial \psi }{\partial d_{1y}}}={\displaystyle \frac{\partial \psi }{\partial d_{0y}}}={\displaystyle \frac{\partial \psi }{\partial d_{1z}}}={\displaystyle \frac{\partial \psi }{\partial d_{0z}}}=0\end{array}$$

(83)

and

$$\begin{array}{c}{\displaystyle \frac{\partial \theta }{\partial d_{1x}}}={\displaystyle \frac{\partial \theta }{\partial d_{0x}}}=0\\ {\displaystyle \frac{\partial \theta }{\partial d_{1y}}}={\displaystyle \frac{-c\theta }{Hc\psi }},{\displaystyle \frac{\partial \theta }{\partial d_{0y}}}={\displaystyle \frac{c\theta }{Hc\psi }}\\ {\displaystyle \frac{\partial \theta }{\partial d_{1z}}}={\displaystyle \frac{-s\theta }{Hc\psi }},{\displaystyle \frac{\partial \theta }{\partial d_{0z}}}={\displaystyle \frac{s\theta }{Hc\psi }}\end{array}$$

(84)

Putting it all together, (84), (83) and (81) transform (79) into

$$\begin{array}{cc}\hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0x}}}=-{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1x}}}={\mathbf{\Xi }}^T\left[\begin{array}{c}0\\ {\displaystyle \frac{-1}{Hc\psi }}\end{array}\right]\hfill \\ \hfill & {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0y}}}=-{\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1y}}}={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{c\theta }{Hc\psi }}\\ 0\end{array}\right]\hfill \\ \hfill & \begin{array}{cc}\hfill {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{0z}}}& =f+C\hfill \\ \hfill {\displaystyle \frac{\partial \mathrm{U}}{\partial d_{1z}}}& =-f+D\hfill \end{array},f={\mathbf{\Xi }}^T\left[\begin{array}{c}{\displaystyle \frac{s\theta }{Hc\psi }}\\ 0\end{array}\right]\hfill \end{array}$$

(85)

Restructuring the equations, we can write the gravity compensation forces in the following form:

$$\begin{array}{c}\hfill {\mathbf{F}}_{\mathbf{g}\mathbf{0}}={\displaystyle \frac{1}{Hc\psi }}\left[\begin{array}{cc}0& -1\\ c\theta & 0\\ s\theta & 0\end{array}\right]\mathbf{\Xi }+\left[\begin{array}{c}0\\ 0\\ D\end{array}\right],{\mathbf{F}}_{\mathbf{g}\mathbf{1}}={\displaystyle \frac{-1}{Hc\psi }}\left[\begin{array}{cc}0& -1\\ c\theta & 0\\ s\theta & 0\end{array}\right]\mathbf{\Xi }+\left[\begin{array}{c}0\\ 0\\ C\end{array}\right]\end{array}$$

(86)

We observe that the two forces consist of an opposite component plus a vertical force specific to the mass of the links supported by each one.

2.7. Remote Centre of Motion

The point in a mechanism that remains fixed under rotation is known as the center of motion (CM). When the CM is not located on the mechanism itself, it is called a remote center of motion (RCM). Robots equipped with RCM are particularly valuable in laparoscopic surgery, as they can adapt to procedural kinematics. Typically, surgeons make small incisions in the abdominal wall through which cannulas (trocars) are inserted, providing access to the inner site. The laparoscopic instruments are then introduced through the trocars and execute a pivoting motion around this fulcrum point (Figure 6), confined within a cone with its vertex at said point. From a kinematic point of view, the entry point serves as the RCM of the robotic manipulator.

Although most surgical robots available have a statically/mechanically fixed RCM, the Double Delta robot can dynamically emulate motion through a user-specified RCM. In other words, the RCM can be altered programmatically to suit the user’s requirements.

By imposing an RCM, the range of motion of the laparoscopic tool is limited to yaw and pitch about the insertion axis, along with a linear in–out motion. Consequently, the degrees of freedom of the robot are reduced to three. Let ${\mathbf{P}}_{\mathbf{rcm}}$ denote the RCM of the manipulator in the base frame. The tool’s axis must always intersect with ${\mathbf{P}}_{\mathbf{rcm}}$. If the tooltip point ${\mathbf{P}}_{\mathbf{T}}$ is known, then the vector ${\mathbf{S}}_{\mathbf{rcm}}={\mathbf{P}}_{\mathbf{T}}-{\mathbf{P}}_{\mathbf{rcm}}$ lies on this axis and is parallel to vector ${\mathbf{S}}_{\mathbf{H}}$. Hence, we can calculate ${\mathbf{S}}_{\mathbf{H}}$ as follows:

$${\mathbf{S}}_{\mathbf{H}}=H{\mathbf{S}}_{\mathbf{rcm}}/\parallel S_{rcm}\parallel$$

(87)

and utilizing (33), we can compute the orientation matrix ${\mathbf{R}}_{\mathbf{T}}$ of the platform, consequently solving the inverse kinematics.

2.8. Workspace Analysis

In this section, we delve into an analysis of the platform’s workspace, considering the robot’s capability to navigate specific points under various orientations with its five degrees of freedom. We determine this reachable workspace by utilizing a sampling-based method involving inverse kinematics.

Initially, we create a 3D regular grid, sampling the space around the tooltip while the platform is at rest. Subsequently, we systematically sample yaw and pitch angles to generate all possible orientations for the tool. Points ${\mathbf{j}}_{\mathbf{0}}$ and ${\mathbf{j}}_{\mathbf{1}}$ on the holder trace two concentric spheres centered at ${\mathbf{P}}_{\mathbf{T}}$. By moving these spheres to each grid point ${\mathbf{W}}_{\mathbf{i}}$, we produce a set of candidate poses for the tool. Each pose is then examined to determine whether it is a valid configuration of the platform. Here we can discern two methods; the first is to use the analytical inverse kinematics of each Delta to produce the full reachable workspace ${\mathbf{W}}_{\mathbf{full}}$, or we can utilize the conical approximation for the Deltas’ workspaces, as presented in Section 2.4, resulting in the approximate reachable workspace ${\mathbf{W}}_{\mathbf{apx}}$.

The full workspace is the true attainable range of motions the robot can acquire, but the approximate workspace is used in the actual analysis and control of the robot and is the one used in the programmatic implementation due to its fast cone inclusion tests. In addition to these two workspaces, when the RCM constraint is applied, the degrees of freedom of the platform drop to “three” and the reachable workspace is contracted. This leads to the RCM workspace ${\mathbf{W}}_{\mathbf{rcm}}$, which is evidently a subset of the approximate workspace. Since the RCM is parametrizable and can thus change, ${\mathbf{W}}_{\mathbf{rcm}}$ is a family of workspaces, dependent on the actual position of the remote center of motion. The maximal (in terms of volume) RCM workspace is presented in Figure 7, along with the full and the approximate reachable workspaces.

In particular, we observe that the full workspace is not symmetric about the xz-plane, as is the case with the other two workspaces, due to the two Deltas being mounted facing different directions (frames ${\mathbf{D}}_{\mathbf{0}},{\mathbf{D}}_{\mathbf{1}}$ are rotated by $2\pi /3$ and $-2\pi /3$ about the $Z_B$ axis—see Figure 2). To produce the previous workspaces, the following robot parameters were used (

Table 1. Robot parameters used for workspace calculation.

Description	Parameter Name	Value
Delta workspace conic approx. radius	r	124 (mm)
Delta workspace conic approx. height	h	118 (mm)
Horizontal offset of tool axis to tool holder	D	78 (mm)
Vertical offset of tooltip to joint ${\mathbf{j}}_{\mathbf{1}}$	${\ell }_T$	405 (mm)
Range of pitch angle	$\Delta \psi$	±60 (deg)
Length of links $L_H$ and $L_5$	H	65 (mm)
Length of link $L_1$	${\ell }_1$	165.5 (mm)
Length of link $L_2$	${\ell }_2$	440 (mm)
Length of link $L_3$	${\ell }_3$	230.5 (mm)
Length of link $L_4$	${\ell }_4$	780 (mm)
Length of parallelogram mechanism	S	142 (mm)

):

To measure the workspaces, we determine their volume by employing an alpha shape reconstruction. This involves utilizing the critical alpha radius that generates a closed, non-convex three-dimensional solid encompassing all the points within the workspace [23]. In the following table, we present the volume of each workspace as computed by the alpha shape, the step used for creating the sampling grid and the dimensions of its bounding box, viz.

Table 2. Volume and extent of the various workspaces.

Workspace	Sampling Step (mm)	Volume (lt)	Bounding Box (mm)
			X	Y	Z
Delta actual	2	3.78	830	885	545
Delta cone	-	1.90	118	118	124
Double Delta full	5	126.39	830	885	545
Double Delta aprx.	2.5	87.58	762	800	527
Double Delta RCM	5	55.22	675	770	525

reveals that the conical approximation of each Delta’s workspace is approximately half of its actual workspace (1.90 vs. 3.78 lt). However, this results in a workspace for the Double Delta that is roughly 30% less than its full range (87.58 vs. 126.39 lt). We can accept this underutilization of the workspace as a deliberate compromise for the swift conical inclusion test in the inverse kinematics solution, along with providing a safety buffer for the robot to prevent links from extending to the boundaries.

We emphasize that the RCM workspace presented in Table 2 pertains to the maximal workspace. To determine this, we conducted a systematic search, correlating the RCM point’s position with its resulting Double Delta workspace. This involved sampling the space surrounding the tool using a regular grid, here with a step of 10 mm, generating a set of candidate RCM points. For each point, we computed the valid robot poses where the tooltip ${\mathbf{P}}_{\mathbf{T}}$ is within ${\mathbf{W}}_{\mathbf{apx}}$. This process yielded a collection of RCM voxels, each associated with a specific workspace volume. A visual representation of this grid, color-coded according to the volume of the corresponding workspace, is presented in Figure 8.

Notice that the maximum workspace volume associated with the RCM voxels is approximately 58 lt, whereas the value presented in Table 2 is recorded as 55.22 lt. This discrepancy arises from our approach to accelerate calculations and ensure the tractability of the search process. Instead of directly calculating the volume of the alpha shape for the workspace of a given RCM point, we opted for a more efficient method. We collected the number of valid points, and by multiplying this count with the voxel volume of the workspace grid, we estimated the volume corresponding to each point’s workspace. This method yields a slightly higher estimate, approximately 5% more, compared to the direct calculation of the alpha shape.

To gain a more profound insight into the stratification of the RCM voxels, we performed an analysis by computing the cumulative histogram of RCM voxels based on their corresponding workspace volumes. This plot provides a comprehensive view of the volume distribution, showing the volume of RCM voxels that have at least a given workspace volume. Essentially, it offers a nuanced understanding of how the workspace volumes are distributed across the RCM voxels. As depicted in Figure 9, the plot illustrates that the volume of RCM voxels with a minimum reachable workspace of 10 lt is approximately 8 lt. This volume decreases to 2 lt for a 40 lt reachable workspace and continues this trend for other values. Notably, a substantial portion of the plot demonstrates linear behavior. Given that the plot is presented on a semi-logarithmic axis, we can infer that the distribution of RCM voxels follows an exponential decay pattern with respect to the workspace volume.

The workspace needed for laparoscopic surgery depends on the procedure. For example, an analysis in robotic-assisted cholecystectomy estimated an overall workspace of about 0.7 lt in the shape of an ellipsoid [24]. In general, the available volume can vary according to the achieved intra-abdominal pressure to sustain a pneumoperitoneum, abdominal compliance, patient body mass index surgical position and the actual surgical field [25]. The normal workspace during laparoscopy ranges from 3 to 6 lt [26]. According to the distribution in Figure 9, this range is covered by 12 to 21 lt of RCM voxels.

2.9. Simulation and Control

To test the previous analysis and develop control algorithms, the system was modeled in Matlab’s Simscape Multibody, which provides a multi-body physics simulation environment for 3D mechanical systems. The platform was initially designed in a commercial CAD/CAM software package (DS Solidworks) and then ported to Simscape (Figure 10). However, its complexity was reduced by considering internal components (gear, etc.) as solid objects rigidly attached to their place. The motion transmission elements were modeled by replacing the rotary joints $j_i,q_i,r_i$ with the respective components in Simscape. The inertia matrix for each link was determined and inserted into the simulation. Note that only the linkage system was modeled. The two driving Delta robots were abstracted as two Cartesian joints located at points ${\mathbf{d}}_{\mathbf{0}},{\mathbf{d}}_{\mathbf{1}}$. The input of the system was the two forces ${\mathbf{F}}_{\mathbf{0}},{\mathbf{F}}_{\mathbf{1}}$ of these two actuators.

The control objective was to track a reference point for the tooltip $p_t$ point under an RCM constraint, i.e., a teleoperation task. To this end, we developed a PID+G controller that regulates the forces ${\mathbf{F}}_{\mathbf{0}},{\mathbf{F}}_{\mathbf{1}}$. The gravity term “G”, as computed by (86), is a feed-forward signal accounting for gravity compensation. This is superimposed with the feedback loop signal from the PID controllers. The two forces are regulated by two different control loops. However, the control gains are the same for both controllers. Furthermore, even though the forces are 3D vectors and the controllers are thus MIMO, they contain only diagonal elements i.e., the cross-coupling input–output terms are set to null. The control equation is given by

$${\mathbf{F}}_{\mathbf{i}}={\mathbf{F}}_{\mathbf{gi}}+K_p{\mathbf{e}}_{\mathbf{di}}+K_{d}D\left({\mathbf{e}}_{\mathbf{di}}\right)+K_i{\int }_0^t{\mathbf{e}}_{\mathbf{di}}$$

(88)

where ${\mathbf{e}}_{\mathbf{di}}={\mathbf{d}}_{\mathbf{iref}}-{\mathbf{d}}_{\mathbf{i}}$ is the error between the points ${\mathbf{d}}_{\mathbf{i}}$ and the reference positions ${\mathbf{d}}_{\mathbf{iref}}$ of these points computed by inverse kinematics, given the reference position ${\mathbf{P}}_{\mathbf{tref}}$ of the tooltip. The derivative term “D” is implemented as a low-pass filter with a cut-off frequency N = 20 Hz. The PID output was also limited by a saturation block to prevent excessive signals from driving the motors. A schematic of the control diagram is presented in Figure 11.

3. Results

The PID controllers were manually tuned with gains set to $K_p$ = 30,000, $K_i$ = 5000 and $K_d$ = 1000. To assess their performance, two tests were conducted. In the first test, a dynamic tracking task was executed in which the objective was to follow a circular reference trajectory with a tooltip. This trajectory, formed by a point moving along the circle at a frequency of 0.33 Hz, was located on the $x-y$ plane with a radius of 50 mm.

For the second task, the reference signal was derived from an actual experiment using the da Vinci surgical robot. Specifically, we extracted kinematic data from the left patient side manipulator during a suturing trial from the JIGSAWS dataset [27]. The data were then used as input to the controller for tracking.

The results of the first experiment are shown in Figure 12.

The aggregate data for both tracking experiments are given in

Table 3. Tracking errors for the two experiments.

	Circle Tracking Experiment
	Distance 1 Error		max			min
	rmse (mm)	max (mm)	X	Y	Z	X	Y	Z
$\mathbf{\Delta }{\mathbf{d}}_{\mathbf{0}}$	0.0503	0.0776	0.0768	0.0105	0.0122	−0.0730	−0.0100	−0.0000
$\mathbf{\Delta }{\mathbf{d}}_{\mathbf{1}}$	0.0522	0.0801	0.0757	0.0133	0.0021	−0.0791	−0.0158	−0.0127
$\mathbf{\Delta }{\mathbf{P}}_{\mathbf{t}}$	0.5872	0.9041	0.8461	0.1445	0.3185	−0.8605	−0.1151	−0.0627
	JIGSAWS suturing tracking experiment
	Distance 1 error		max			min
	rmse (mm)	max (mm)	X	Y	Z	X	Y	Z
$\mathbf{\Delta }{\mathbf{d}}_{\mathbf{0}}$	0.1334	2.0777	0.1912	0.1071	1.9920	−0.1514	−0.0781	−2.0776
$\mathbf{\Delta }{\mathbf{d}}_{\mathbf{1}}$	0.1334	2.0846	0.1808	0.1014	1.9918	−0.1948	−0.1450	−2.0804
$\mathbf{\Delta }{\mathbf{P}}_{\mathbf{t}}$	0.2556	2.0849	1.4602	0.8749	2.0205	−0.9919	−0.6418	−1.9550

¹ L2 distance of Δ∗.

. We see that both the rms error and the maximum tracking error of the tooltip point ${\mathbf{P}}_{\mathbf{t}}$ are well below 1 mm, similarly for the upper and lower errors on each dimension X-Y-Z separately.

As a further step to investigate the performance and stability of the controlled system, we performed a nonlinear Bode analysis, following the related concept as presented by Pavlov et al. in [28]. Assuming that the closed-loop system is convergent, i.e., it has a unique, globally asymptotically stable solution called the steady-state solution, by applying a harmonic excitation with varying frequencies $\omega$ and amplitudes $\alpha$, the system admits a unique periodic solution ${\overline{x}}_{\omega \alpha }\left(t\right)$. We note that this solution need not be harmonic and depends both on the frequency and on the amplitude. If we denote its maximum absolute value at $\omega$ by $B(\omega ,\alpha )$, then the ratio ${\gamma }_{\alpha }\left(\omega \right)=B(\omega ,\alpha )/\alpha$ can be regarded as an amplification gain. Taking the maximum gain over a range of amplitudes $(\underline{\alpha },\overline{\alpha })$, we can consider ${{\rm Y}}_{(\underline{\alpha },\overline{\alpha })}\left(\omega \right)={\sup }_{\alpha }{\gamma }_{\alpha }\left(\omega \right)$, $\alpha \in (\underline{\alpha },\overline{\alpha })$. Plotting ${\rm Y}$ over $\omega$, we can visualize the non-linear Bode plot for the closed-loop system. This expresses the maximum amplification gain per frequency. For our system at hand, we performed this procedure by providing harmonic excitations at each input separately (X-Y-Z) in a frequency range of 0.5 to 30 Hz. The amplitudes ranged from 25 mm to 125 mm. By computing the amplification gain, we obtain the diagram seen in Figure 13. Since our system is MIMO, we also include the cross-coupling terms, i.e., X-Y, X-Z, Y-X, Y-Z, Z-X and Z-Y.

We notice that the amplification in the diagonal gains is relatively small, up to approximately 8–9 Hz. However, at higher frequencies, the system demonstrates considerable damping. The cross-coupling terms similarly display notable attenuation at lower frequencies, although this diminishes as the frequency increases. However, even in the most extreme case, it remains below −10 dB. This suggests a substantial level of decoupling between the inputs and outputs.

It is worth mentioning that when examining the frequency spectrum of the suturing trials in the JIGSAWS dataset, over 99% of the signals’ power is concentrated below 1 Hz. In this light, the Bode diagram underscores the system’s propensity to follow the low-frequency activity of actual surgical maneuvers.

4. Discussion

Compared to other research platforms, the Double Delta seems to present better tracking performance. For example, the mean tracking error for the MOPS robot is reported to be between 1.2 mm to 2.7 mm [12]. Similarly, tracking tests using a cascade controller for the Raven II platform present a mean translational error of 1.55 mm [29]. We note, however, that since the Raven II robot is cable driven, the estimation of its pose, which was used in the controller, is particularly difficult as it is affected by elasticity, friction and tensioning of the cables. The estimation error of its end-effector is in the range of 10 to 25 mm [29], which is an order of magnitude larger than the tracking error of the controller. Efforts to reduce this uncertainty are also reported in the literature using vision-based neural networks [30]. However, this setup increases the complexity of the system, adding overhead and cost.

Similar problems are met with the da Vinci research kit. The dVRK is also cable driven, something that impacts its kinematic estimation accuracy. The tool tip estimation error is reported to be up to 1.02 mm [10], whereas trajectory tracking experiments report a mean error of 2.05 mm [31]. The Double Delta bypasses the problems exhibited with cable-driven actuation by using rigid links connected with revolute joints directly driven by the servomotors. Since the links are connected in parallel, the mechanism presents better rigidity than a single serial link. This also allows the use of thinner links, hence, reducing the overall weight. Furthermore, the parallel structure of the Double Delta enables it to exert more force per weight, leading to less bulky motors and links and, ultimately, to a more compact size.

5. Conclusions

This paper explores the theoretical aspects of the third iteration of the Double Delta surgical robot. We conducted an analysis of both the forward and inverse position kinematics of the platform, along with a velocity analysis, focusing on the interface points of the upper linkage with the two driving Delta robots. Using an RCM constraint, we derived the inverse motion equations and provided a detailed description of its workspace, demonstrating its capability to meet the requirements of various robotic surgery procedures. Simulation experiments based on a physics-based model for control design exhibited promising tracking performance in two experiments. As a part of future work, we aim to implement the control design on the actual system and explore more sophisticated control strategies that leverage the parallel kinematic structure of the design.