1. Introduction
Minimally invasive surgery has been a continuing medical trend because it can reduce the pain and the recovery time of patients. Robotic surgery has emerged as a solution for minimally invasive surgery. Diverse continuum robot manipulators are being developed for laparoscopic or endoscopic surgery [
1]. The most well-known surgical robot system is the da Vinci system (Intuitive Surgical Inc., CA), which has been successfully used in various surgeries in the past decade. Its efficiency has been verified in various ways [
2].
The da Vinci Si and Xi have wire-driven robotic arms for multiport surgery. Robot arms access the surgical target through multiple ports [
3], but many studies have tried to reduce the number of ports. The da Vinci Si can perform single-port surgery using new curved instruments [
4]. Recently, the da Vinci SP was developed specifically for single-port surgery [
5]. Another type of wire-driven robot is the SPORT Surgical System (TITAN Medical, Canada) [
6], which has triple-segment robot arms with a slit structure and can carry out smooth motion in a single port. Furthermore, flexible surgical robot arms that can be attached to endoscopes were developed to perform endoluminal surgery [
7], and an interleaved continuum-rigid manipulator was studied to improve dexterous workspace and accurate actuation [
8].
We are developing a wire-driven surgical robot for single-port surgery, as shown in
Figure 1a. Two major features of this robot are a joint structure to secure stiffness and double segments to fit the working volume in a single port. Most wire-driven robots are symmetrically controlled with rotating pulleys. In such robots, all wires can be controlled independently for precise movement and other features, as shown in
Figure 1b. The proximal joints and distal joints each have two degrees of freedom (DoFs), and the whole body is capable of rotational and translational motion. Consequentially, the end-effector can achieve a six-DoF motion.
There are two well-known structures of a snake-like surgical robot arm: the slit structure and the joint structure [
9]. The slit structure is a flexible pipe with slits in various patterns, as shown in
Figure 2a. The SPORT System is a representative example. The slit structure can make a smoothly curved configuration, and the model is simplified to the curvature model. However, this structure does not have enough stiffness to perform some general surgical operations, including sutures.
An articulated robot arm with a joint structure consists of orthogonally stacked joints, as shown in
Figure 2b. One example is the da Vinci Si. A joint structure cannot be curved in a perfect curvature configuration, but it has stronger stiffness than a slit structure. The set of stacked joints that share the connected wires is called a segment. One segment with orthogonally stacked joints can bend with a two-DoF motion. In single-port surgery, dual robotic arms have to be able to maintain a triangular configuration like a crab’s claw, and an articulated arm with multiple segments is essential to establish the working space, as shown in
Figure 2c.
The motion of the distal segments is affected by the motion of the proximal segments because the wires that control the distal segment pass through all stacked joints (see
Figure 2c). The blue wire is connected to the median linkage with a wire crimp terminal and controls only the proximal segment. The red wire controls the distal segment and passes through all joints. This phenomenon is called the coupling problem, which must be solved to control robot arms with multiple segments.
The accuracy of the robot model heavily influences the robot’s performance. The robot arm has a tiny structure with a diameter of less than 6 mm. The robot arm operates inside the body, and it is hard to attach encoders or sensors to the arm to measure joint angles or trace the end-effector’s position and orientation. This type of robot is usually controlled by open-loop control based on a mathematical model, unlike general industrial robot arms. The wire driving length has to be calculated from the robot model and a path generation algorithm according to an arbitrary input from the master device in real-time without any feedback from the joint configuration. Therefore, a comparison study about modeling approaches for tendon-actuated continuum robots was carried out [
10].
Most snake-like robot arms have been analyzed with a constant-curvature model for convenience [
11]. Recently, a variable-curvature model was derived to improve the accuracy of the curvature model [
12]. The curvature model works well for a continuum body such as the slit structure, but it does not fit well with a joint structure. The driving wire length is calculated with modeling error, and wire slack behavior may occur between the joints, where the joints are not able to bend to the desired angle or maintain stiffness. In this paper, a more accurate kinematic model was derived to fit an articulated robot arm with a joint structure, and effective path generation algorithms are proposed to solve the inverse kinematics.
One challenging problem is how to solve inverse kinematics from the task space to the wire-length space and generate an actuation path in real-time. A kinematic model of a wire-driven robot arm usually consists of three spaces called the task space, joint space, and wire-length space, as shown in
Figure 3. The task space includes the position and orientation of the end-effector, which are given from the master device with the surgeon in real-time. The joint space deals with the joint configuration, which includes the joint angles, translation, and rotation of the robot arm. The wire-length space includes the wire-driving length from the motors.
The general approach with the curvature model is to solve the inverse kinematics from the task space to the wire-length space at once. When the coupling problem is solved in multiple segments, however, this approach is restricted by geometrical assumptions [
13,
14]. In addition, an accurate kinematic model requires more complicated calculations. The numerical method is an effective way to solve inverse kinematics due to the complexity of the forward kinematic model with a joint structure. The sequential quadratic programming (SQP) method is one of the most successful general methods to solve the nonlinearly constrained minimization problem [
15]. A combination of SQP and the iterative Newton–Raphson algorithm was used to solve the inverse kinematics of a humanoid robot with redundancy [
16]. Recently, an open-source library (TRAC-IK) was developed and distributed for solving inverse kinematics with SQP [
17]. As an alternative to the optimization path generator, a path generator was designed using a PID feedback control algorithm. In this method, solving for inverse kinematics is considered to be a feedback control problem, and the analytical solution is derived by inverting the differential kinematics. A short calculation time can be guaranteed because there is no iteration. The stability of the closed-loop inverse kinematics algorithm for redundant robots was verified in the discrete time domain [
18]. The closed-loop inverse kinematics algorithm showed advantages in singular value filtering and task priority strategy in comparative experiments of a whole-arm manipulator (WAM) robot [
19].
In this study, the solution for inverse kinematics was divided into two steps: one from the task space to the joint space and one from the joint space to the wire-length space. First, an accurate forward kinematic model from the joint space to the task space was derived while considering the joint structure. Then, the joint space solution could be obtained through inverse kinematics. The inverse kinematic model from the joint space to the wire-length space was derived using a modified curvature model. Finally, the two inverse kinematics were connected by substituting the calculated values, which could effectively generate an accurate actuation path corresponding to each wire-driving length in real-time. In this project, tungsten wire was considered to be a rigid structure, and the friction and elongation of the wire were negligible. The robotic arm could be operated only by position control.
This paper proposes two algorithms to solve the inverse kinematics from the forward kinematic model. One of them is a numerical method with sequential quadratic programming (SQP), and the other uses differential kinematics with a control algorithm. In the numerical method, the constraint logic for the mechanical limits of the robot arm works well in real-time, even when a human operator mistakenly manipulates the master device to an excessive value. Compared to another method, the numerical method can easily respond to changes in robot structure, since only the forward kinematic model needs to be replaced without Jacobian or gain tuning. However, calculation speeds due to iteration should always be considered and designed. The differential kinematic method is much faster and much more accurate than the numerical method. However, it cannot restrict the actuation value to the mechanical limits.
The remainder of this paper is organized as follows.
Section 2 presents a derivation of the forward kinematic model of the joint structure using frame transformation and compares the results to the curvature model.
Section 3 describes the inverse kinematic model from the joint space to the wire-length space with the modified curvature model. The advantages of this model in preventing slack and solving the coupling problem are discussed.
Section 4 introduces the two path generation algorithms. The strengths and weaknesses of each algorithm are discussed with simulation results.
Section 5 presents experimental results with a surgical robot prototype to verify the feasibility of the path generators. The last section gives a conclusion and discusses future work.
2. Forward Kinematic Model from Joint Space to Task Space
The forward kinematic model was derived using a classical approach with rigid body transformation. Moving frames were attached to each joint, and the relative position and orientation between frames can be described with a homogeneous transformation matrix. The arm has multiple one-DoF revolute joints that are stacked. The axes of the stacked joints are arranged orthogonally to increase the DoFs and secure the wire path [
20]. The forward kinematic model can be obtained from the sequential product of the transformation matrices.
Figure 4 shows a diagram of a single-joint section. The relationship between the
i − 1th frame and the
ith frame is described by Equation (1) for frames from 1 to 3, where
ϕ is the assigned angle of the joint axis, whose parameter is initially determined;
θ is the bending angle of the joint, which changes with the joint configuration; and
h is the height of the joint axis from the base plane. To produce the transformation matrix of the next joint independently, the assigned angle
ϕ has to return to its initial value:
An example is shown in
Figure 5. The arm can achieve a six-DoF motion with double segments. The translation motion of the base is
γ, and the rolling motion is
ψ. The proximal segment has two joints and can simultaneously bend with
θ1 and
θ2 in orthogonal directions. The distal segment has four joints and can bend with
θ3 and
θ4 in orthogonal directions. Each set of two distal joints is coupled. It is assumed that the coupled joints equally share a bending angle.
The thickness between joint bases is
lt, and the lengths of the medial linkage and end-effector are
lm and
le, respectively. The transformation matrices for the translation motion and rolling motion are Equation (2) and Equation (3), respectively. The matrix
G in Equation (4) just moves the frames by the length of the components. The transformation matrix from the inertial frame to the end-effector’s frame can be obtained from the product of the matrices, as shown in Equation (5). In this work, the product of the exponential was used to derive the forward kinematics, and the configuration parameters are arranged in
Table 1. As a result, the position and orientation of the end-effector can be calculated, as shown in Equation (6), where the subscripts of the matrix represent the number of rows and columns. The orientation is expressed as
z–y–x Euler angles for the roll, pitch, and yaw. This is the exact forward kinematic model of the articulated robotic arm with a joint structure.
In another example, the forward kinematics of a single-segment robotic arm with four joints is compared between the curvature model and the joint model, as shown in
Figure 6a,b. Both orthogonal joints simultaneously bend at equal angles. The position difference between the two models is within about 1 mm, as shown in
Figure 6c. The maximum orientation difference is about 8° according to the bending angle, as shown in
Figure 6d. When surgeons control the robotic arm using master devices in real-time, they are more sensitive to the orientation error than the position error because it is more visible [
21]. An accurate kinematic model helps to make operators more comfortable.
3. Inverse Kinematic Model from Joint Space to Wire-Length Space
The inverse kinematics of a wire-driven robotic arm represent the physical relationship between the task space and the wire-length space. Inverse kinematics are divided into two steps: one from the task space to the joint space and one from the joint space to the wire-length space. First, the inverse kinematic model from the joint space to the wire-length space is derived from a geometric approach.
The conventional curvature model starts with an assumption that the arc length at the center is always conserved. However, in the joint structure, the virtual arc length at the center varies with the bending angle.
Figure 7 shows a comparison between the curvature model and the joint model. The wire length at the center is
lc, and the outer and inner wire lengths of the bending joint are
lr and
lp, respectively. The distance between the joint axis and the wire is
r, and the length of all wires is the same as the linkage height
2h before the joint bends. Each wire length is calculated based on the conventional curvature model shown in Equation (7). The wire length during pulling and that during releasing are symmetric. In this study,
h is 1 mm, and
r is 2.45 mm.
In the joint structure, the wire length model has to be modified, as shown in Equation (8). The wire lengths during pulling and releasing are not symmetric with respect to the initial wire length in this case. If the wires are driven in accordance with the curvature model, the wires are pulled less and released more compared to the modified model, as shown in
Figure 8. Then, slack occurs in the driving wires, and the joint cannot bend to the desired angle or secure stiffness. The modified model with asymmetric lengths for the joint structure guarantees more accurate motion without slack.
The wire length at the center becomes shorter according to the bending angle. In the general case, wires to control the jaws of the end-effector are located at the center of the robotic arm. The jaws of the end-effector are used to not fully close or open in an arm configuration with a large bending angle. This model shows that the driving wire length at the center to actuate the end-effector needs compensation for the bending angle of the joints:
Another advantage of this model is that the compensation length for the coupling problem is solved at the same time. In the stacked joint structure, the wires affect all joints that they pass through. When the wire length to actuate a joint is calculated, the bending angles of all joints that the wire has previously passed through have to be considered. Since this wire length model is a function of only the joint angles, all wire lengths are naturally decided according to the joint configuration.
Figure 9 shows views of the distal section from the example in
Figure 5. In this case, the arrangement of joints is A–B (proximal)/C–D–D–C (distal). The distal joints are assembled pairs. The two pairs of blue boxes indicate joints, and the red dashed–dotted line across the two blue boxes represents the joint axis. Wires 1, 3, 5, and 7 are connected to proximal joints. Wires 2, 4, 6, and 8 are connected to distal joints through the proximal joints. The driving length of wire 1 is derived as shown in Equation (9):
where sgn() is the sign function.
Wire 1 can control only the proximal joint angle and does not affect the distal joint angle at all. Wire 4 is different from wire 1, and its driving length consists of two parts: the compensation length for proximal joints and the driving length for distal joints, as shown in Equation (10). The compensation length is geometrically determined from the angles of the proximal joints. If the wire length does not properly compensate for the proximal joint angle, the distal joints may bend to an unexpected configuration.
5. Experiments
The two path generators for the optimization method and the Jacobian matrix with PID were embedded in a prototype system, as shown in
Figure 1. The diameter of the robot arms was 6 mm, and the tungsten wire had a dimension of
ϕ 0.36 mm and a cross-sectional structure of 7 × 19, which means the number of cores and strands in multifilamentary wires. For both methods, we used one Dell Precision T5810 desktop with an Intel Xeon 3.6GHz processor (12 logical cores, 15-MB cache) and 32 GB of DDR4 RAM for inverse kinematics calculations. In addition, we used one BeckHoff C9900 machine with an Intel I7 (2 logical cores, 2.4-GHz clock speed, 6-MB cache) and 16.0 GB of RAM for motor control software. The operating system installed on both machines was 64-bit Microsoft Windows 7 Enterprise.
For the matrix and other mathematical computations, we used the C++ standard math library for trigonometry calculations and the Intel Math Kernel Library (MKL) 10.3 with Armadillo 8.5 (based on Lapack 3.8.0 and OpenBLAS 0.2.20) for matrix multiplication. To acquire a set of reference paths for both methods, a pair of haptic devices (Omega 6 from Force Dimension) was used (left and right). Each of them generates seven-DoF tip poses (x, y, z, roll, pitch, yaw, and grasper) with a frequency range of 3.5 to 4 kHz.
In the case of the optimization path generator, the Non-Linear Optimization library (NLOpt) 2.4.2 was used to search for the optimal inverse kinematics solution (the joint angles). In NLOpt, we used the simplex-based COBYLA method for optimal solution exploration within the bounds of the given solution space ([−67 mm, 150 mm], ±2π, ±π/4, ±π/4, ±π/2, and ±π/2 for translation, rotation, and individual joint angles θ1 to θ4, respectively). The maximum search time per iteration was 10 ms, and the search operation could be terminated if the objective function decreased below .
The objective function should consider the scale difference between the metric and angular units. Therefore, this work applied a left-invariant vector field to translate the metric and angular distances to an equal normalized space [
30]. Each search operation started from the most recent valid solution set. In addition, the search was terminated and returned the current optimized solution with a “FAILURE” tag if NLOpt could not find a solution with an object value that was lower than
(because of the joint angle limitations or kinematic singularity).
For the path generator with the Jacobian and PID control algorithm, we used MKL and Armadillo for matrix calculation, matrix transpose, rank computation, and singular value decomposition (SVD) calculation for the matrix pseudoinverse. The system calculation time was measured by the Chrono standard library as 680 μs or less. We applied the same coefficient set to the MATLAB simulation in
Table 2.
Measurements were performed with an electromagnetic (EM) tracking system (Aurora, Northern Digital Inc., Canada). The field generator was set up around the robot arm, and six-DoF sensors were attached to the tip of the robot arm and the reference points, as shown in
Figure 20. The position accuracy was 0.48 mm with a 95% confidential interval of 0.88 mm, and the orientation accuracy was 0.30° with a 95% confidential interval of 0.48°. The actual motion of the robot driven through the path generators was measured according to the desired path. The same desired path as in
Figure 11 was applied to the system. The control group was the motion that was operated by the path generator based on the constant-curvature model of the arm configuration and wire length. The articulated robotic arm was simplified as the constant-curvature model. The driving wire length was generated symmetrically based on the curvature model.
The motions of the end-effector controlled with the two proposed path generators followed the desired trajectory well, but the motion with the constant-curvature model did not, as shown in
Figure 21. The blue solid line presents the end-effector’s orientation with the optimization path generator, and the green dotted line shows the results from the path generator with the Jacobian matrix and PID control algorithm. The red dashed–dotted line represents the results from the constant-curvature model, and the black dashed line is the desired orientation path. The two proposed path generators showed similar results, even near 120 s. Both of the experimental results with the two path generators could not reach the desired values due to the physical limitation of the joint structure. However, the path generator with the Jacobian matrix and PID control algorithm generated commands that were beyond the limitations of the joint structure. If these commands are repeated, the wires and joints may suffer damage.
The experimental results showed the accuracy of the proposed kinematic model and the validity of the two path generator algorithms. The forward kinematic model of the articulated joint robot arm with frame transformation and the symmetric wire length model for the joint structure were more precise than the constant-curvature model. The two proposed path generators solved the inverse kinematics efficiently and generated accurate actuation paths for open-loop control in an actual system.
6. Conclusions
A wire-driven surgical robot arm with a joint structure has a tiny structure and operates inside the human body. In general, the robot is operated through open-loop control. Furthermore, the robot has to operate according to an arbitrary path in real-time and not a predefined path. A path generator based on an accurate kinematic model is vital to the performance of the robot. In this study, the accuracy of the kinematic model was improved with the product of the transformation matrices with moving frames and an asymmetrical wire-driving model for the joint structure. The proposed wire-driving model also solved the coupling problem due to the double-segment structure for single-port surgery.
This paper presented two path generators that solved for inverse kinematics: an optimization path generator and a path generator with a Jacobian matrix and PID control algorithm. The optimization path generator solved the inverse kinematics using the SQP numerical optimization method. This path generator operated within a computing loop time of the 10 ms. Operators did not feel the delay, but considering that additional algorithms will be implemented, the computational time due to iteration may be worrisome. The logic of constraints on the mechanical limits of robot arms worked well in real-time even when the master device generated the desired position and direction at an excessive value.
The path generator with the Jacobian matirx and PID control algorithm derived the inverse kinematic solution using differential kinematics and a feedback control algorithm. The important features, in this case, were the null space solution for the coupled constraints due to the articulated joint structure and back-calculation to prevent sudden divergence. This path generator was much faster and much more accurate than the optimization path generator. However, it could not restrict the actuation value to the mechanical limits. The experimental results showed performance improvement due to the accurate kinematic model and the validity of the path generators embedded in a surgical robot prototype. In future work, a path-regulation algorithm will be designed for a second path generator.