Identification of Inertial Parameters for Position and Force Control of Surgical Assistance Robots

Abstract

Surgeries or rehabilitation exercises are hazardous tasks for a mechanical system, as the device has to interact with parts of the human body without the hands-on experience that the surgeon or physiotherapist acquires over time. For various gynecological laparoscopic surgeries, such as laparoscopic hysterectomy or laparoscopic pelvic endometriosis, Uterine Manipulators are used. These medical devices allow the uterus to be suitably mobilized. A gap needs to be filled in terms of the precise handling of this type of devices. In this sense, this manuscript first describes the mathematical procedure to identify the inertial parameters of uterine manipulators. These parameters are needed to establish an accurate position and force control for an electromechanical system to assist surgical operations. The method for identifying the mass and the center of mass of the manipulator is based on the solution of the equations for the static equilibrium of rigid solids. Based on the manipulator inertial parameter estimation, the paper shows how the force exerted by the manipulator can be obtained. For this purpose, it solves a matrix system composed of the torques and forces of the manipulator. Different manipulators have been used, and it has been verified that the mathematical procedures proposed in this work allow us to calculate in an accurate and efficient way the force exerted by these manipulators.

1. Introduction

In recent years, the fields of application of robotics have grown increasingly. Thus, in addition to typical industrial applications (pick and place, arc or spot welding, machine tending, etc.), nowadays robots are being adopted in fields such as rescue robots [1], robots that replace human operators in inaccessible or dangerous environments [2], assistance robots [3,4], medicine [5,6], etc.

In the field of medicine, the design of robots to assist or perform surgeries has been increasing for more than two decades. Today it is possible to find robots in various procedures, such as heart [7], thoracic [8], endonasal [9], gastrointestinal [10], urological [11], and laparoscopic hysterectomy [12,13] surgeries. The use of computational methods such as finite element analysis makes it possible to accelerate the design and validation of new solutions in the field of surgery [14].

A good example of a medical robot is the Da Vinci Robotic System [15], and despite its initial high cost, with proper planning and training of professionals, its economic viability can be demonstrated [16]. This robot presents several advantages, such as the elimination of the surgeon’s hand tremor, less invasive and safer surgeries, shorter operating time, smaller incisions, etc. [17], making a completely autonomous surgeries possible in the future [18].

An example of this class of surgery is the robotic laparoscopic hysterectomy. It is a total or partial removal of the uterus. The surgery is performed by doing a few small incisions in the abdomen of the patient through which the robot arms are inserted [19].

Because the Da Vinci robot needs a solid support for stitching or incisions, it is necessary the use of uterine manipulator during the intervention [20,21,22]. The manipulator is introduced through the vagina, and it is manually handled by a second surgeon [23].

At the Universitat Politècnica de València, together with the Hospital General Universitario of Valencia, a specific purpose robot has been developed. The robot operates the uterine manipulator, preventing the surgeon from spending many hours in an unergonomic positions holding a weighty manipulator.

The developed assistance robot performs the necessary movements to position and orient the extreme of the uterine manipulator during the surgical intervention. For this purpose, the manipulator is connected to the robot end effector.

In addition, the manipulator needs a support point. This element is attached to the stretcher (see Figure 1). This support point is used to limit lateral movements and to protect the vagina’s internal walls from possible undesirable forces.

Normally in robotic applications it is necessary to have a precise position control to ensure that the robot has a small error tracking in the desired trajectory. In the case of surgeries, a very precise force control is also required since excessive force could cause very serious injuries to the patients.

To avoid possible injuries from exerting excessive forces during the use of a manipulator, a sensor has been installed at the end of the robot, in order to indirectly measure the exerted force at the extreme of the actuator. The idea of placing the force sensor at the end of the manipulator has been discarded since it would require a certified sensor in this environment to guarantee the safety of the patient. This entails a high cost and low flexibility regarding its use for other manipulators.

To obtain the estimation of this force, the static equations of the rigid solid have been used [24]. These equations require the inertial parameters of the manipulator. However, these characteristics are not usually provided by the manufacturers of these medical devices. In addition, since different models of manipulators can be used, it is necessary to provide a mathematical method for calculating the physical characteristics of the manipulators.

Only the mass and the center of mass (COM) are needed to calculate the manipulator’s force since it moves very slowly. This kind of motion makes inertial forces and moments and Coriolis forces negligible.

An identification method has been developed to obtain the inertial parameters. It uses a least squares method [25,26,27] and the force sensor to measure the force at various points.

Once the inertial parameters have been calculated, the force exerted by the extremes of the manipulator can be obtained from the force measurements obtained by the sensor.

The objective of this paper is to describe the development and the experimentation of a low-cost surgical assistance robot (SAR) to help in surgeries like laparoscopic hysterectomy. To reduce the risk of the surgery, the force that this robot is exerting on the patient must be controlled. In this manuscript, a mathematical method is presented to obtain an estimation of this force.

In addition, it is necessary to emphasize that the force at the end of the uterine manipulator depends on the inertial parameters of the system, especially the mass and the location of the (COM) of the manipulator. Therefore, the article also presents a procedure for automatic identification of these inertial parameters.

The paper is organized as follows—Section 2 presents the kinematic model of the robot and how to obtain the coordinates of the end of the manipulator. Section 3 describes the procedure to determine the inertial parameters, and the calculation of the force at the end of the manipulator. Section 5 presents the SAR used in this work, and Section 6 presents the obtained results for the actual SAR. Finally, in Section 7, the main conclusions and future work are presented.

2. Kinematic Model of Surgical Assistance Robot

2.1. Design Specifications

The objective of the project is to control, using a low-cost robot controlled by a limited capacity computational system, with a simple design and with a surgery manipulator with 3 degrees of freedom (DOF), a lineal movement of the uterine manipulator in the X axis and two circular movements at the Y and Z axis of the Figure 2 and to be able to control exerted force at the extremes of the manipulator over its environment.

A cartesian robot has been chosen with three linear kinematic pairs (P of Figure 1) perpendicular to each other. At the end point of the cartesian robot (O₃), a sensor has been arranged to know the forces performed at the extreme of the manipulator. A universal joint has been interposed between the sensor and the uterine manipulator, which initially increases the number of DOF to 5. However, 2 DOFs have been restricted by setting a support point at O_G that limits the lineal movement along Y₀ and Z₀, turning them into twists.

2.2. Denavit–Hartenberg Parameters

Denavit–Hartenberg [28] is a methodical procedure to assign reference systems to the links of a robot to know the position and orientation of each part of the robot and to establish a coordinate system.

The cartesian robot includes three active prismatic joints (q₁, q₂ and q₃), which represent the three linear movements along the cartesian axes, and two passive universal joints (q₄ and q₅). The parameters q₁, q₂ and q₃ are the active variables and represent the three linear movement of prismatic joints, while q₄ and q₅ are the passive variables and represent the generalized coordinates of the universal joint. The manipulator is considered like a straight stick—parameter L of the

Table 1. Denavit–Hartenberg (DH) parameters of the robot.

j	α1j	a1j	d1j	θ1j
1	0	q1	0	−π/2
2	−π/2	q2	0	−π/2
3	0	q3	0	0
4	q4	0	0	π/2
5	q5	d	L	0

indicates distance between of center O₄ and O₅ at X₅ axis, parameter d it the distance between center O₄ and O₅ Z₅ axis.

The value of the two passive revolute joints of the universal joint (q₄ and q₅) is solved by equating the unit director vector of the points O₄ and O₅, with the unit director vector between points O₄ and O_G. According to Equation (1), using this equation, Equations (2) and (3) are obtained with the value of the passive variables of the SAR.

$$\frac{\overrightarrow{r_{O_4}}-\overrightarrow{r_{O_G}}}{\left|\overrightarrow{r_{O_4}}-\overrightarrow{r_{O_G}}\right|}=\frac{\overrightarrow{r_{O_4}}-\overrightarrow{r_{O_5}}}{\left|\overrightarrow{r_{O_4}}-\overrightarrow{r_{O_5}}\right|}$$

(1)

$$q_5={\sin }^{-1}\left(\frac{O_{G_x}-q_3}{\left|\overrightarrow{r_{O_4{O}_G}}\right|}\right)$$

(2)

$$q_4={\cos }^{-1}\left(\frac{-b\pm \sqrt{b^2-4ac} }{2a}\right)$$

(3)

where:

$$a=d^2·\left|\overrightarrow{r_{O_4{O}_G}}\right|+m^2·L^2\left(O_{G_z}-q_2\right)$$

(4)

$$b= -2m·L^2\left(O_{G_z}-q_1\right)$$

(5)

$$c=L^2{\left(O_{G_z}-q_1\right)}^2-d^2·{\left|\overrightarrow{r_{O_4{O}_G}}\right|}^2$$

(6)

$$m= \sqrt{{\left(O_{G_y}-q_2\right)}^2+{\left(O_{G_z}-q_1\right)}^2}$$

(7)

In this way, the coordinates of the end of the manipulator can be obtained by applying the constraint equations of point O₅:

$$X_5=Lsin\left(q_4\right)\cos \left(q_5\right)+dsin\left(q_4\right)\sin \left(q_5\right)+q_3$$

(8)

$$Y_5=Lsin\left(q_5\right)-dcos\left(q_5\right)+q_2$$

(9)

$$Z_5=Lcos\left(q_4\right)\cos \left(q_5\right)+dcos\left(q_4\right)\sin \left(q_5\right)+q_1$$

(10)

3. Identification of Inertial Parameters

The identification of inertial parameters is an experimental procedure to obtain all or some inertial parameters such as mass, location of the center of the mass, inertial moments, inertial tensor, etc. It is based on rewriting the inverse dynamics equations in a linear way with respect to the parameters to be identified. These techniques have been satisfactorily applied in the field of robotics and others, for example in the next contributions [29,30,31].

Industrial robots allow the execution of programs that carry out a sequence of movements required to perform a task. In standard robot movements, only the position of the end of the actuator is usually controlled. However, there are applications in which it is necessary to precisely control the interaction that robot has with the environment. In these cases, not only a position control is needed but, but also a force control.

Force-controlled movements have been available in experimental robotic systems for the last 2 decades. But industrial robot controllers did not provide force control until recently, and furthermore, this force control was packaged for manufacturing tasks.

In the most demanding cases, where movements must be carried out with precision and variability in both position and force, an external measurement is needed to cope with the unmodeled geometric variations.

In this way, force sensors provide information about the force/torque interaction between the robot and the environment so that control strategies programmed in the robot allow for force-controlled movements. This force feedback can be essential for many applications that involve the manipulation of objects with restrictions, that is, docking and assembly tasks, in which two or more parts must be assembled, ensuring adequate contact with each other [32]. Other examples are surface machining tasks such as milling, grinding, deburring or polishing.

Another case in which force control is essential is in medical applications, such as those discussed in this work. Since the end of the uterine manipulator has direct contact with the uterus of patient, great forces must be avoided, since this could cause serious injury to the patients.

On the other hand, since the uterine manipulator rests on a support point, it is necessary to know the relationship between the forces read by the end of the robot and the forces that are being applied to the patient. To resolve this relation, it is necessary to first identify the inertial parameters of the uterine manipulator.

During surgeries, the manipulator is moved slowly until it reaches the desired position and orientation. During its movement, inertial or Coriolis effect are insignificant. The most important forces that actuate are—the gravity force due to the mass, the force at the support point and the reaction at the support point. By knowing the mass and the COM of the manipulator, it is possible to resolve the force at the end point.

The only link that is affected by the method of identification is the uterine manipulator, considering the reaction at the support point. The nomenclature system has been simplified, by calling point O₄ as A, O_G as B, O_COM as C and O₅ as D, as is shown in Figure 3.

The manipulator is mounted on the cartesian robot by means of a universal joint. Despite the fact that is capable to transmit forces at three axes and torsional moment, this moment has not been considered because it is not relevant.

The identification method of mass and COM is based on equations for the static equilibrium of a rigid solid (sum of forces and moments is equal to 0).

$${\displaystyle \sum }\overrightarrow{{}_{ }{}^{0}M_B}=\overrightarrow{r_{BA}}\times \overrightarrow{F_A}+\overrightarrow{r_{BC}}\times \overrightarrow{F_C}=0$$

(11)

$$\overrightarrow{F_C}= \left\{\begin{array}{ccc}0& -m·g& 0\end{array}\right\}$$

(12)

Point A transmits forces at three axes and point B only transmits forces perpendicular at vector $\overrightarrow{r_{BA}}$.

$${\displaystyle \sum }\overrightarrow{{}_{ }{}^{0}M_B}=\left\{\begin{array}{c}{r}_{B{A}_y}·F_{A_z}-r_{B{A}_z}·F_{A_y}+r_{B{C}_z}·m_C·g\\ r_{B{A}_z}·F_{A_x}-r_{B{A}_x}·F_{A_z}\\ r_{B{A}_x}·F_{A_y}-r_{B{A}_y}·F_{A_x}-r_{B{C}_x}·m_C·g\end{array}\right\}$$

(13)

$$\overrightarrow{r_{BC}}= \overrightarrow{r_{BA}}·\frac{\left|\overrightarrow{r_{AB}}\right|-d}{\left|\overrightarrow{r_{AB}}\right|}$$

(14)

$${\displaystyle \sum }\overrightarrow{{}_{ }{}^{0}M_B}=\left\{\begin{array}{c}{r}_{B{A}_y}·F_{A_z}-r_{B{A}_z}·F_{A_y}+\overrightarrow{r_{BA}}·\frac{\left|\overrightarrow{r_{AB}}\right|-d}{\left|\overrightarrow{r_{AB}}\right|}·m_C·g\\ r_{B{A}_z}·F_{A_x}-r_{B{A}_x}·F_{A_z}\\ r_{B{A}_x}·F_{A_y}-r_{B{A}_y}·F_{A_x}-\overrightarrow{r_{BA}}·\frac{\left|\overrightarrow{r_{AB}}\right|-d}{\left|\overrightarrow{r_{AB}}\right|}·m_C·g\end{array}\right\}=0$$

(15)

Mass and COM have been solved, calculating the moment at point B and setting the moment equal to 0. The force vector of point C has been replaced by vector of the force of gravity, and a system of equations with 2 unknowns and 2 equations has been obtained (17).

$$M=m·\left|\overrightarrow{r_{AC}}\right|$$

(16)

$$\left\{\begin{array}{cc}{r}_{B{A}_z}·g& \raisebox{1ex}{$-r_{B{A}_z}·g$}\!\left/ \!\raisebox{-1ex}{$\left|\overrightarrow{r_{AB}}\right|$}\right.\\ r_{B{A}_x}·g& \raisebox{1ex}{$-r_{B{A}_x}·g$}\!\left/ \!\raisebox{-1ex}{$\left|\overrightarrow{r_{AB}}\right|$}\right.\end{array}\right\}·\left\{\begin{array}{c}m\\ M\end{array}\right\}=\left\{\begin{array}{c}{r}_{B{A}_z}·F_{A_y}-r_{B{A}_y}·F_{A_z}\\ r_{B{A}_x}·F_{A_y}-r_{B{A}_y}·F_{A_x}\end{array}\right\}$$

(17)

The system of equations obtained is linearly dependent, but the system is solved using n points with different absolute distances $\overrightarrow{r_{AB}}$, obtaining the matrix system of (18), which is solved as described in (19) using the pseudo-inverse of Moore–Penrose [26], whose operation is represented with a superscript symbol +.

$$g{\left\{\begin{array}{cc}{r}_{B{A}_{z}i}& \raisebox{1ex}{$-r_{B{A}_{z}i}$}\!\left/ \!\raisebox{-1ex}{${\left|\overrightarrow{r_{AB}}\right|}_i$}\right.\\ r_{B{A}_{x}i}& \raisebox{1ex}{$-r_{B{A}_x}$}\!\left/ \!\raisebox{-1ex}{${\left|\overrightarrow{r_{AB}}\right|}_i$}\right.\end{array}\right\}}_{2nx2}·{\left\{\begin{array}{c}m\\ M\end{array}\right\}}_{2x1}={\left\{\begin{array}{c}{r}_{B{A}_{y}i}·F_{A_{z}i}-r_{B{A}_{z}i}·F_{A_{y}i}\\ r_{B{A}_{yi}}·F_{A_{x}i}-r_{B{A}_{x}i}·F_{A_{y}i}\end{array}\right\}}_{2nx1}$$

(18)

$${\left\{\begin{array}{c}m\\ M\end{array}\right\}}_{2x1}={{\left(g{\left\{\begin{array}{cc}{r}_{B{A}_{z}i}& \raisebox{1ex}{$-r_{B{A}_{z}i}$}\!\left/ \!\raisebox{-1ex}{${\left|\overrightarrow{r_{AB}}\right|}_i$}\right.\\ r_{B{A}_{x}i}& \raisebox{1ex}{$-r_{B{A}_x}$}\!\left/ \!\raisebox{-1ex}{${\left|\overrightarrow{r_{AB}}\right|}_i$}\right.\end{array}\right\}}_{2nx2}\right)}_{2x2n}}^{+}·{\left\{\begin{array}{c}{r}_{B{A}_{y}i}·F_{A_{z}i}-r_{B{A}_{z}i}·F_{A_{y}i}\\ r_{B{A}_{yi}}·F_{A_{x}i}-r_{B{A}_{x}i}·F_{A_{y}i}\end{array}\right\}}_{2nx1}$$

(19)

4. Calculation of External Force at End of the Manipulator

It is necessary to know force at the end point of the manipulator to prevent excessive forces exerted to the patient. To calculate the force at the extreme point of the manipulator, equilibrium static equations of the rigid solid have been used.

Knowing the mass and the COM of the manipulator, as well as the measured force in point A and with the force of the sensor in point A, it is possible to know the force exerted at the extremes of the manipulator. Using again the equations of static equilibrium of a rigid solid, we have a total of 5 unknows—3 axes of forces at the extreme point, 2 reactions at the support point—one parallel to the plane generated between the vector $\overrightarrow{r_{AB}}$ and the gravity vector that is called F_B1 and a second force perpendicular to the vector $\overrightarrow{r_{AB}}$ and belonging to the anterior plane (Figure 4), that is called F_B2.

$$\overrightarrow{u_{F_{B1}}}=\frac{\overrightarrow{g}\times \overrightarrow{r_{AB}}}{\left|\overrightarrow{g}\times \overrightarrow{r_{AB}}\right|}$$

(20)

$$\overrightarrow{u_{F_{B2}}}=\frac{\left(\overrightarrow{g}\times \overrightarrow{r_{AB}}\right)\times \overrightarrow{r_{AB}}}{\left|\left(\overrightarrow{g}\times \overrightarrow{r_{AB}}\right)\times \overrightarrow{r_{AB}}\right|}$$

(21)

$$\overrightarrow{F_B}=F_{B1}·\overrightarrow{u_{F_{B1}}}+F_{B2}·\overrightarrow{u_{F_{B2}}}$$

(22)

A matrixial system has been created with two components, the first is the summation of moments at point A and the second is the summation of forces, creating an overdetermined linear system with 5 unknowns and 6 equations. In this way, the system of Equation (13) can be solved using the Moore–Penrose pseudo-inverse.

$${\left\{\begin{array}{c}\overrightarrow{r_{BA}}\times \overrightarrow{F_B}+\overrightarrow{r_{DA}}\times \overrightarrow{F_D}\\ \overrightarrow{F_B}+\overrightarrow{F_D}\end{array}\right\}}_{6x1}={\left\{\begin{array}{c}\overrightarrow{r_{AC}}\times \overrightarrow{F_C}\\ -\overrightarrow{F_A}-\overrightarrow{F_C}\end{array}\right\}}_{6x1}$$

(23)

Besides, we have checked that it is possible to eliminate one of the equations without modifying the result and we have chosen to eliminate the Y axis of the summation of the moments (24), because this axis has been considered to provide less information.

$$\{\begin{array}{ccccc}0& {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{z}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{z}}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{Z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\\ {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{x}}}& 0& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{x}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{x}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 1& 0& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{x}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{x}}}}\\ 0& 1& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 0& 0& 1& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{z}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\end{array}\}·\left\{\begin{array}{c}{F}_{D_x}\\ F_{D_y}\\ F_{D_z}\\ F_{B1}\\ F_{B2}\end{array}\right\}=\left\{\begin{array}{c}-r_{A{C}_z}·F_{C_y}\\ r_{A{C}_x}·F_{C_y}\\ -F_{A_x}\\ -F_{A_y}-F_{C_y}\\ -F_{A_z}\end{array}\right\}$$

(24)

5. Methodology: Implementation in the Actual Robot

5.1. Surgery Assistance Robot

The designed robot consists of 3 linear guides perpendicular to each other with a repeatability of ±0.05 mm. Each linear guide is equipped with an electrical servomotor. To measure displacement, servomotors have an absolute step encoder of 12 bits per turn and 20 bits of multiturn and an electromagnetic brake. The nominal torque is 0.4 Nm, the nominal speed is 3000 rpm and the input voltage is 24 V.

The control unit of the SAR is an embedded PC with an ARM^® 800 Mhz microprocessor, 256 Mb RAM and 256 Mb Flash. In addition, the embedded PC has Ethernet, EtherCAT, USB ports, digital and analog inputs and outputs.

An OnRobot HEX-6 force sensor is installed in the end element of the robot. This has a precision of 0.2 N for the X and Y axes, and 0.8 N for the Z axis.

The six DOF force sensor is an OnRobot HEX-6 coupled between the Y axis and the end effector of the robot (see Figure 5). The OnRobot HEX-6 force sensor provides a resolution of 0.2 N for the X and Y axes, and 0.8 N for the Z axis.

5.2. Calculation of Inertial Parameters

To calculate the mass of the manipulator and its COM it is necessary to modify the absolute value of the vector $\overrightarrow{r_{AB}}$. This is achieved by moving the manipulator in the direction of the support point. It is necessary to bear in mind that when these movements are carried out, inertial forces and moments and friction forces of the manipulator appear on the support point. Executing a movement parallel to XZ plane and cancelling the height difference between the manipulator and the turning point $\left(r_{B{A}_y}=0\right)$, force components on the X₀ and Z₀ axes disappear. Measuring the value of the force on the Y₀ axis at the force sensor on the Equation (19), the Equation (25) is obtained.

$${\left\{\begin{array}{c}m\\ M\end{array}\right\}}_{2x1}={\left(g{\left\{\begin{array}{cc}{r}_{B{A}_{z}i}& \raisebox{1ex}{$-r_{B{A}_{z}i}$}\!\left/ \!\raisebox{-1ex}{${\left|\overrightarrow{r_{AB}}\right|}_i$}\right.\\ r_{B{A}_{x}i}& \raisebox{1ex}{$-r_{B{A}_x}$}\!\left/ \!\raisebox{-1ex}{${\left|\overrightarrow{r_{AB}}\right|}_i$}\right.\end{array}\right\}}_{2nx2}\right)}^{+}·{\left\{\begin{array}{c}0·F_{A_{z}i}-r_{B{A}_{z}i}·F_{A_{y}i}\\ 0·F_{A_{x}i}-r_{B{A}_{x}i}·F_{A_{y}i}\end{array}\right\}}_{2nx1}$$

(25)

It is not possible to perform matrix calculations on the embedded PC. The identification of the mass has been reduced to a two-dimensional system using local coordinates of the manipulator (Figure 3), obtaining the linear Equation (27).

$${\displaystyle \sum }\overrightarrow{{}_{ }{}^{0}M_B}=\left(\left|\overrightarrow{r_{AB}}\right|-\left|\overrightarrow{r_{AC}}\right|\right)·F_{C_{y^{\prime }}}+F_{A_{y^{\prime }}}·\left|\overrightarrow{r_{AB}}\right|=0$$

(26)

$$m·g-\frac{\left|\overrightarrow{r_{AC}}\right|}{\left|\overrightarrow{r_{AB}}\right|}·m·g=F_{A_y}$$

(27)

In this way, the method of least squares can be applied with two constants [33], obtaining the following solution:

$$b_0+b_1·X=Y$$

(28)

where:

$$\{\begin{array}{c}{b}_0=m·g\hfill \\ b_1=m·g·\left|\overrightarrow{r_{AC}}\right|\hfill \\ X=\frac{1}{\left|\overrightarrow{r_{AB}}\right|}\hfill \\ X=\frac{1}{\left|\overrightarrow{r_{AB}}\right|}\hfill \end{array}$$

5.3. Calculation of External Forces

As discussed above, in surgeries such as robotic laparoscopic hysterectomies, it is critical to control the force exerted on the patient. In the case of the assistive robot we have developed, the force at the end of the manipulator can be obtained by reading the force sensor and the following equation:

$$\left\{\begin{array}{c}{F}_{D_x}\\ F_{D_y}\\ F_{D_z}\\ F_{B1}\\ F_{B2}\end{array}\right\}={\left(\begin{array}{ccccc}0& {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{z}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_z}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\\ {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{x}}}& 0& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{x}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{x}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 1& 0& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{x}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{x}}}}\\ 0& 1& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 0& 0& 1& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{z}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\end{array}\right)}^{-1}·\left\{\begin{array}{c}-r_{A{C}_z}·F_{C_y}\\ r_{A{C}_x}·F_{C_y}\\ -F_{A_x}\\ -F_{A_y}-F_{C_y}\\ -F_{A_z}\end{array}\right\}$$

(29)

Finally, it should be noted that, because the embedded PC does not allow the execution of matrix calculation, the matrix Equation (29) has been transformed into multiple single-line equations. To do this, the transposed adjoint matrix has been calculated, multiplied by the input data vector and divided by the determinant of the matrix.

$$\frac{adj({(\begin{array}{ccccc}0& {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{z}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{z}}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\\ {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{x}}}& 0& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{x}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_x}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 1& 0& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{x}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{x}}}}\\ 0& 1& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 0& 0& 1& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{z}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\end{array})}^T)·(\begin{array}{c}-r_{A{C}_z}·F_{C_y}\\ r_{A{C}_x}·F_{C_y}\\ -F_{A_x}\\ -F_{A_y}-F_{C_y}\\ -F_{A_z}\end{array})}{\left|\begin{array}{ccccc}0& {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{z}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{z}}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{z}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\\ {\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{y}}}& -{\mathrm{r}}_{{\mathrm{AD}}_{\mathrm{x}}}& 0& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{x}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{y}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{x}}}}-{\mathrm{r}}_{{\mathrm{AB}}_{\mathrm{x}}}·{\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 1& 0& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_x}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{x}}}}\\ 0& 1& 0& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{y}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{y}}}}\\ 0& 0& 1& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}1}_{\mathrm{z}}}}& {\mathrm{u}}_{{\mathrm{F}}_{{\mathrm{B}2}_{\mathrm{z}}}}\end{array}\right|}$$

(30)

6. Results and Discussion

The method introduced in this paper has been applied to several manipulators with different masses and COM, and the results are depicted in

Table 2. Results of the identification of manipulators.

	Real		Calculated		Error
Name	Mass (kg)	COM (m)	Mass (kg)	COM (m)	Mass (kg)	COM (m)
Manipulator 1	1.611	0.115	1.538	0.1129	−4.53%	−1.83%
Manipulator 2	2.593	0.103	2.5602	0.1055	−1.26%	2.43%
Manipulator 3	3.198	0.125	3.2208	0.1303	0.71%	4.24%
Manipulator 4	4.159	0.1456	4.206	0.1398	1.13%	−3.98%
Mean					−0.99%	0.21%
Standard deviation					2.58%	3.78%

. The COM has been measured from the point O₄, in the direction given by X₅. For the identification process, point O₄ has been arranged as close as possible to point O_G, without any difference in length in Y₀ axis between both points, and a movement has been made in the negative direction X₅, until reaching almost the manipulator limit, leaving a margin of 5 cm (Figure 6).

After calculating the inertial parameters for each manipulator, a few random trajectories have been performed. The reaction force appearing at the sensor has been calculated, and it has been compared with recorded data, measuring the method’s error and the effect of friction and the rest of inertial parameters, which were not previously considered (Figure 7).

To check the correct running of calculation of the manipulator’s end point, a known mass has been applied at the end point, O₅, and it has been checked if the method could solve the correctly applied force (

Table 3. Identification of the extra mass applied at the end of the manipulator.

Mass Applicated (kg)	Mass Calculated (kg)	Error
0.51	0.5061	−0.765%
1.03	1.013	−1.650%
2.04	2.030	0.490%

), with an acceptable error (Figure 8).

Finally, changing forces have been applied to the end point, and they have been measured with an external unidirectional force sensor and the result obtained by calculating the force at the extreme point has been compared with the value measured by the external sensor used (Figure 9).

To measure the method’s error, the absolute relative error has been used [34] (21), using the calculated forces (${\tau }_{id{n}_i}$), the measure forces (${\tau }_i^{\ast }$), and the mean of the measured forces ($\overline{\tau }$). The absolute relative error represents the differences between the calculated value and the measured value with respect to the differences between the mean value and the measured value. The error obtained is 19.79%.

$${\epsilon }_{ra}=\frac{{{\displaystyle \sum }}_i\left|{\tau }_{id{n}_i}-{\tau }_i{}^{\ast }\right|}{{{\displaystyle \sum }}_i\left|{\tau }_i{}^{\ast }-\overline{{\tau }^{\ast }}\right|}$$

(31)

7. Conclusions

In robotic laparoscopic hysterectomies, a uterine manipulator is needed to assist with sutures or incisions. This manipulator is introduced through the vagina, and a surgeon must hand it manually during the entire surgery.

This paper has presented the problems related to the use of a robot for the operation of uterine manipulators. In this case, it is necessary to have a very precise control of the force exerted on the patients.

The developed SAR is equipped with a force sensor. However, because there is a holder on which the manipulator is supported, it is not trivial to obtain the actual force at the extreme of the manipulator by reading the force sensor.

This paper has shown that the manipulator’s mass and COM can be obtained using the equations of static equilibrium. In addition, because the control unit of the robot cannot perform matrix calculations, the identification procedure has been simplified to a linear equation that is solved by a least squares method.

In addition, a method has been developed that allows us to obtain an estimation of the force at the end of the uterine manipulator by means of the forces read by the sensor.

Finally, several experimental tests of parameters’ identification and force estimation have been carried out. These tests validate the proposed solution, providing very satisfactory results.

Although a result of almost 20% in the absolute relative error seems a high value, Figure 9 shows how the maximum difference does not exceed 2 N. When the surgeons were consulted, they considered that this error is acceptable in this type of surgeries and that this does not pose a risk to the patient.

In future works, the SAR will be tested in simulation classrooms to allow hysteroscopic training using surgical simulators. In a second phase, a user interface design will be designed. It will be a user-friendly interface to make the robot easy, efficient and pleasant to operate. Finally, the SAR will be validated through a series of clinical trials.