Solving the Time-Varying Inverse Kinematics Problem for the Da Vinci Surgical Robot

A dialytic-elimination and Newton-iteration based quasi-analytic inverse kinematics approach is proposed for the 6 degree of freedom (DOF) active slave manipulator in the Da Vinci surgical robot and other similar systems. First, the transformation matrix-based inverse kinematics model is derived; then, its high-dimensional nonlinear equations are transformed to a high-order nonlinear equation with only one unknown variable by using the dialytic elimination with a unitary matrix. Finally, the quasi-analytic solution is eventually obtained by the Newton iteration method. Simulations are conducted, and the result show that the proposed quasi-analytic approach has advantages in terms of accuracy (error < 0.00004 degree (or mm)), solution speed (<20 ms) and is barely affected by the singularity during intermediate calculations, which proves that the approach meets the real-time and high-accuracy requirements of master–slave mapping control for the Da Vinci surgical robots and other similar systems. In addition, the proposed approach can also serve as a design reference for other types of robotic arms that do not satisfy the Pieper principle.


Introduction
In recent years, medical robots have increasingly been used as fundamental surgical instrument/equipment [1], among which the Da Vinci system developed by Intuitive Surgical Company is the most successful [2].The Da Vinci robotic system consists of three components: the surgeon console (master manipulators), the patient trolley (slave manipulators), and the imaging system [3].The surgeon manipulates two master handles at the master remote console, which can acquire high-resolution, binocular, three-dimensional, magnified views of the operative field as compared with open surgery [4].However, this kind of master-slave operation mode poses a great challenge to the real-time and accurate performance of the master-slave mapping control, among which the inverse kinematics problem is the first obstacle.The Da Vinci system and similar surgical robot systems satisfy the following two features: (1) contain mechanical structures to generate Remote Center of Motion [5] (RCM, a fixed point on the active arm, there are two methods to generate RCM based motion: (a) virtual RCM, (b) physically constrained RCM [6]), and the structure can be simplified; (2) do not satisfy the Pieper principle.
At present, the main methods of robotic inverse kinematics include the analytic method and the numerical method.The analytic approach uses vector, spinor or Lie algebra to obtain results [7][8][9], which can only obtain satisfied solutions in limited robotic configurations that should satisfy the Pieper principle [10,11], etc.However, to imitate the surgeon operation, the manipulators of most thoracic and abdominal surgery robots, including the Da Vinci system, contain rotational/translational kinematic pairs, tilt angle joints, and offset joints at the tip (artificial wrist part) that are designed for additional flexibility, which is beyond the solution capability of the analytic approach.
One feasible strategy is to simplify the kinematic model to acquire analytical results.For example, Ai et al. and Podsedkowski et al. [12,13] obtained an approximate solution by separating the position and orientation of the slave manipulators, the solution deviation of which needs to be manually eliminated by experienced surgeons via visual inspection.Zhang et al. [14] solved the inverse kinematics of a 4-DOF surgical robot using the elimination method, and Mezouar et al. [15] used unit dual quaternions to model the kinematics, however, outcomes of their studies benefited strongly from the simplicity of the arm configuration.In addition, Fu et al. [16,17] obtained an approximate solution based on the differential transformation that increases error feedbacks to compensate the cumulative error, the final precision of which can reach 0.18 mm.
Another way to solve the inverse kinematics problem of the surgical robot that does not satisfy the Pieper principle is the Jacobian matrix-based numerical method [18].However, this method involves a Jacobian matrix, which may lose versatility due to its singularity problem as well as the low convergence speed of the iterative solutions.To improve the numerical solution performance, a numerical method based on the position increment of robotic joints was proposed [18].Specifically, to make this method suitable for the inverse kinematics analysis of a robotic manipulator with an offset wrist, Bu et al. [19] used the method of decoupling DOFs based on a cut-off point that is limited by the manipulator configuration.In addition, modern methods such as the neural network method [20], the genetic algorithm [21], the electromagnetism-like mechanism [22,23], the hybrid electromagnetism-like mechanism [24], etc., have been gradually explored for possible solutions of the inverse kinematics of surgical robots.However, these methods either require a large amount of sample data or suffer from poor convergence speed; therefore, these methods are not suitable for the rapid development of a master-slave manipulator in surgical robots.
To address the lack of efficient and accurate inverse kinematics solutions for the Da Vinci system, we propose a quasi-analytic solution method that involves dialytic elimination and the Newton iteration method.The dialytic elimination method [25] is used to transform the high-dimensional equations into a nonlinear equation containing only one unknown variable, which is then solved by the Newton iteration method.In applying this method to the inverse kinematics solution of slave manipulator, we show dual superiorities: (a) barely affected by the singularity problem and (b) maintain rapidity.
The steps of our proposed method as follows: (1) simplify the structure of RCM, using an equivalent mechanism to take the place of the RCM mechanism.(2) establish the coordinate system, this step can be realized by the D-H (Denavit-Hartenberg) method or combine the characteristics of the structure to make sure there are more origins of the coordinate system can be set up at the same point.(3) obtain the mathematical model of forward and inverse kinematics via the matrix transformation method.Then, transform the high-dimensional equations into a nonlinear equation containing only one unknown parameter using the dialytic elimination method, and solve it by the Newton iteration method.
The rest of this paper is organized as follows: Section 2 presented the kinematics analysis of the Da Vinci system; Section 3 solves the inverse kinematics by the Newton iteration method; Section 4 Appl.Sci.2019, 9, 546 3 of 17 analyzes our solution and compared it with other methods via a simulation; Section 5 concludes the paper.

Forward Kinematics Analysis
During surgery using the Da Vinci medical robot, dual articulated arms (slave manipulators) work as the right and left hand of the surgeon, respectively.The coordinate frame of the master/slave manipulator is set the same as the coordinate frame of human sight, i.e., the world coordinate frame.Therefore, in Figure 1, the origin of the base coordinate frame of the slave manipulator is located at the bottom of the robot frame.The Z 0 -axis is upward, along the robot frame.

Forward Kinematics Analysis
During surgery using the Da Vinci medical robot, dual articulated arms (slave manipulators) work as the right and left hand of the surgeon, respectively.The coordinate frame of the master/slave manipulator is set the same as the coordinate frame of human sight, i.e., the world coordinate frame.Therefore, in Figure 1, the origin of the base coordinate frame of the slave manipulator is located at the bottom of the robot frame.The Z0-axis is upward, along the robot frame.
Before surgery, the angles of passive joints M, N, and P and the position of the M joint on the frame are adjusted manually and then remain fixed during surgery.The joints that mainly assist the surgeon are the active joints of the slave manipulator: Q, E, G, K, H, I, and J in Figure 1b.
The DOFs of the slave manipulator includes four DOFs: l0, θ1, θ2 and θ3.The parts 6, 7, and 8 in the active arm belong to the parallelogram mechanism (i.e., these three parts cannot move freely but under the motion constraint of the parallelogram mechanism in Figure 1c.Then only parts 4, 5, 8 and 9 can move freely in the slave manipulator, i.e., part 5 rotates around part 4, there is rotation between part 6 and part 5, and there is rotation between part 9 and part 8. Therefore, the entire slave manipulator will have 6 DOFs when the additional 3 DOFs of the H joint are considered.

Mathematical Model of Forward Kinematics
According to the previous analysis, an active arm coordinate system is established in Figure 2. The origin of the base coordinate x0y0z0 (0-coordinate frame) of the active arm is established at the joint Q between the passive and active arms, the direction of which is the same as that of the frame coordinate X0Y0Z0.Before surgery, the angles of passive joints M, N, and P and the position of the M joint on the frame are adjusted manually and then remain fixed during surgery.The joints that mainly assist the surgeon are the active joints of the slave manipulator: Q, E, G, K, H, I, and J in Figure 1b.
The DOFs of the slave manipulator includes four DOFs: l 0 , θ 1 , θ 2 and θ 3 .The parts 6, 7, and 8 in the active arm belong to the parallelogram mechanism (i.e., these three parts cannot move freely but under the motion constraint of the parallelogram mechanism in Figure 1c.Then only parts 4, 5, 8 and 9 can move freely in the slave manipulator, i.e., part 5 rotates around part 4, there is rotation between part 6 and part 5, and there is rotation between part 9 and part 8. Therefore, the entire slave manipulator will have 6 DOFs when the additional 3 DOFs of the H joint are considered.

Mathematical Model of Forward Kinematics
According to the previous analysis, an active arm coordinate system is established in Figure 2. The origin of the base coordinate x 0 y 0 z 0 (0-coordinate frame) of the active arm is established at the joint Q between the passive and active arms, the direction of which is the same as that of the frame coordinate X 0 Y 0 Z 0 .Figure 2 shows the six DOFs of the active arm in the slave manipulator, namely five rotating joints: q1, q2, q4, q5, q6, and a translational joint: q3.
Six transform matrixes frame from x1y1z1 (1-coordinate frame) to x6y6z6 (6-coordinate frame) can be obtained as follows: where i jT means transform matrix from the i-coordinate frame to the j-coordinate frame; { }x, { }y and { }z represent the rotation angle around the x-axis, y-axis and z-axis, respectively; { } x , { } y and { } z denotes the translational movement along the x-axis, y-axis and z-axis, respectively.Then, the pose matrix of the active arm at the endpoint of the slave manipulator can be obtained as follows: where R and P are the orientation and position of the slave manipulator endpoint, respectively.Substituting Equation (1) into Equation (2) yields Equation (3): Figure 2 shows the six DOFs of the active arm in the slave manipulator, namely five rotating joints: q 1 , q 2 , q 4 , q 5 , q 6 , and a translational joint: q 3 .Six transform matrixes frame from x 1 y 1 z 1 (1-coordinate frame) to x 6 y 6 z 6 (6-coordinate frame) can be obtained as follows: where i j T means transform matrix from the i-coordinate frame to the j-coordinate frame; { } x , { } y and { } z represent the rotation angle around the x-axis, y-axis and z-axis, respectively; { } x , { } y and { } z denotes the translational movement along the x-axis, y-axis and z-axis, respectively.
Then, the pose matrix of the active arm at the endpoint of the slave manipulator can be obtained as follows: where R and P are the orientation and position of the slave manipulator endpoint, respectively.Substituting Equation (1) into Equation (2) yields Equation (3): Appl.Sci.2019, 9, 546 5 of 17

Mathematical Model of Inverse Kinematics
In the case of inverse kinematics, the known pose matrix is 0 6 T in Equation ( 3), and the unknowns are q 1 , q 2 , q 3 , q 4 , q 5 and q 6 .

Left-multiplying the matrices
x on both sides of Equation (3) yields Equation ( 4): To simplify the complexity of the formula, we construct a unitary matrix [26] U as follows.After introducing the matrix U, the variables in the 0 6 T can be replaced by the Euler formula.
In addition, we know that E = U −1 U. Left-multiply the inverse of unitary matrix U −1 and right-multiply the matrix U on both sides of Equation ( 4).Then, multiply the E matrix between these two matrices to transform Equation (4) into the following: Here, 5 and C 6 are constant matrices or parameter matrices related to the structure of the robot.In addition, X j (j = 1, 2, 4, 5, 6) is as follows: where T Xj (j = 1, 2, 4, 5, 6) represents the results of the transform matrix of q j rotating around the z-axis, which undergoes the left-multiplication of U −1 and right-multiplication of U. Let x j = cos(q j ) + isin(q j ), x j −1 = cos(q j ) − isin(q j ); thus, T Xj can be expressed as follows: Let Y j represents the result of the transform matrix q j along the y-axis, which undergoes the left-multiplication of U −1 and right-multiplication of U, and x j = q j ; then, Y j can be expressed as follows: Then the dimensions of matrix A and matrix B are both 4 × 4. Each element in matrix A is a function containing x 1 , x 2 , and x 3 , while the elements in matrix B are functions containing x 4 , x 5 , and x 6 .In addition, corresponding elements in matrix A and matrix B are equal, i.e., Aij = Bij.The specific contents of the elements are detailed in Appendix A.
To realize element elimination, we evaluated elements in matrix A and B that do not contain x 6 and x 6 −1 : B 13 , B 14 , B 23 , B 24 , B 33 and B 34 to establish Equation (10): To further eliminate x 4 and x 5 , the linear combination of x 4 and x 5 , and their reciprocals are treated as new variables, i.e., X = [x 4 x 5 , T , which contains nine variables.Because there are only six equations in Equation ( 10), three more equations are required, which are attainable by conducting nonlinear computations on the elements in Equation ( 10), as shown in Equation (11).
Then, by combining Equations ( 10) and ( 11): The matrix [D(x 1 , x 2 , x 3 )] 9×9 contains only the variables x 1 , x 2 , and x 3 since X is nonzero.The condition that Equation ( 12) has a solution is that the determinant of [D(x 1 , x 2 , x 3 )] 9×9 equals zero.The following equation, which contains x 1 , x 2 , and x 3 , can be obtained provided that |D(x 1 , x 2 , x 3 )| = 0: where C' j is a constant related to the robot's parameters.Considering that x 1 , x 2 , x 4 , x 5 , and x 6 can be defined as x j = cos(q j ) + isin(q j ) and x j −1 = cos(q j ) − isin(q j ), then x 1 , x 2 , x 4 , x 5 , and x 6 are all non-zero.
Therefore, x 2 can be expressed by x 1 according to Equation (13): To represent x 3 by x 1 and x 2 , we construct the equations A L4 = A 23 A 14 + A 13 A 24 + A 33 A 34 and B L4 = B 23 B 14 + B 13 B 24 + B 33 B 34 .Let A L4 = B L4 , then Equation ( 15) contains only three variables, x 1 , x 2 , and x 3 , therefore, x 3 can be expressed by x 1 and x 2 as: Because both A L2 and A L3 are functions of x 1 , x 2 and x 3 , by combining A L2 = B L2 with A L3 = B L3 , the following can be obtained: Therefore, Similarly, by combining A 13 = B 13 and A 14 = B 14 with Equation ( 16), the following can be obtained: Therefore, x 5 can be represented by x 1 , x 2 , and x 3 : Combined with Equations ( 16) to (20) and A 11 = B 11 , x 6 can finally be represented by x 1 : where b j is a constant related to the robot's parameters.
From Equations ( 16) to ( 21), the variables x 2 , x 3 , x 4 , x 5 , and x 6 can be represented with the variable x 1 .Let A 24 and B 24 be equal, then: By solving Equation ( 22), x 1 can be solved, then x 2 , x 3 , x 4 , x 5 , and x 6 can be subsequently solved by Equation ( 16) to (21).Specifically, we know from Equation ( 14) that x 2 = ± f 1 (x 1 ), therefore, F(x 1 ) will have two expressions: Equation ( 23) will be electively solved according to the minimum norm of the distance the joint moved.

Inverse Kinematics Solution
From the derivation of the forward and inverse kinematics of the Da Vinci slave manipulator, it is known that the key to solving inverse kinematics is to solve the nonlinear Equation (22).

Inverse Kinematics Solution Method
The Newton-Raphson method is an efficient numerical iterative approach to solve nonlinear equations in real and complex domains.In this paper, we construct the following iterative formula:

Initial Value of the Inverse Kinematics Solution
The motion ranges of each joint in the Da Vinci slave manipulator are as follows: θ 1 from −90 • to 90 • , θ 2 from −135 • to 0 • , l IH from 0 to 315 mm, θ 3 from −180 • to 180 • , θ 4 from −90 • to 90 • , and θ 5 from −90 • to 90 • .In the previously defined x 1 = cos(q 1 ) + isin(q 1 ), q 1 is the rotation angle of the first joint with a range of [−π/2, π/2].A finite number of values are obtained by extracting values within these available ranges at a certain angle interval (our subsequent calculation results show that a less than 5 • angle interval is acceptable) to solve the forward kinematics, the results of which are stored as the initial value database for the inverse kinematics solution.By selecting the data from the initial value database that is the least squares approximate to the required inverse kinematics as the initial values, the Newton iteration method will successfully converge.

Inverse Kinematics Solution Process
According to the inverse kinematics of the Da Vinci slave manipulator and the derived solution process, a detailed solution flow chart is constructed in Figure 3.In Figure 3, T end is the pose matrix at the endpoint of the robot (i.e., 0 6 T), ε is the precision of the solution, θ 0 is the initial value, h is the initial step value, and X_s represents the values of each joint at previous positions.The value k is used to determine whether both forms of F(x 1 ) have been solved.

Parameters of the Da Vinci Slave Manipulator
According to Figure 4, the size parameters of the Da Vinci slave manipulator are shown in Table 1.

Parameters of the Da Vinci Slave Manipulator
According to Figure 4, the size parameters of the Da Vinci slave manipulator are shown in Table 1.

Solution of Proposed Method
As shown in Table 2 (the form is referenced by the book: An Introduction to Error Analysis [27]), the input values are a set of arbitrarily selected joint parameters within the effective range of joint space.The outputs in Table 2 are the calculated values by our proposed method.h represents the step size of the solution, and the error is calculated as follows: In this paper, the solution precision of the Newton-Raphson method is 1E-10 ε = 1E-7 for Tend, θ0 takes the minimum value of −90° within the feasible region, and the initial step size is h0 = 20°.The test carried on CodeBlocks 16.01 based on the hardware that a PC platform running with an Intel (R) Core (TM) i5-4460 CPU, clocked at 3.2 GHz (memory: 4 GB).Table 1.Structure parameters of the Da Vinci slave manipulator.

Solution of Proposed Method
As shown in Table 2 (the form is referenced by the book: An Introduction to Error Analysis [27]), the input values are a set of arbitrarily selected joint parameters within the effective range of joint space.The outputs in Table 2 are the calculated values by our proposed method.h represents the step size of the solution, and the error is calculated as follows: In this paper, the solution precision of the Newton-Raphson method is 1E-10 ε = 1E-7 for T end , θ 0 takes the minimum value of −90 • within the feasible region, and the initial step size is h 0 = 20 • .The test carried on CodeBlocks 16.01 based on the hardware that a PC platform running with an Intel (R) Core (TM) i5-4460 CPU, clocked at 3.2 GHz (memory: 4 GB).
We also use the same input to run the procedure on the "R12CCPU-V" MELSEC iQ-R series C language controller from Mitsubishi Electric and VxWorks 6.9 operating system, the results are list in Table 3.In order to verify the proposed method, we build a simulation model in Adams.Firstly, we gave a random movement for the simulation model, then we obtained the matrix of the end pose from the simulation result (Figure 5a), which included the position and the orientation.Secondly, the matrixes were imported to the algorithm we proposed as input.Thirdly, we used our solution to drive the robot model.In the final results, depicted in Figure 5b, we see that the curve obtained by our algorithm is very close to the target curve.This means the proposed method is of high accuracy.In order to verify the proposed method, we build a simulation model in Adams.Firstly, we gave a random movement for the simulation model, then we obtained the matrix of the end pose from the simulation result (Figure 5a), which included the position and the orientation.Secondly, the matrixes were imported to the algorithm we proposed as input.Thirdly, we used our solution to drive the robot model.In the final results, depicted in Figure 5b, we see that the curve obtained by our algorithm is very close to the target curve.This means the proposed method is of high accuracy.

Discussion of the Solution
As noted in the previous section, the main focus of the solution includes the solution accuracy, real-time performance (solution speed) and the singularity problem (based on the joint position solution or not).
(1) Solution accuracy Table 2 shows that for any input joint parameter, whether an integer or a decimal value accurate to nine decimal places, the error between the calculated values (output) and the given values (input) of six joint parameters of the slave manipulator is within 0.00004 degree (or mm), which demonstrates that the proposed approach for the Da Vinci surgical robot inverse kinematics is capable of finding effective solutions with high accuracy.

Discussion of the Solution
As noted in the previous section, the main focus of the solution includes the solution accuracy, real-time performance (solution speed) and the singularity problem (based on the joint position solution or not).
Appl.Sci.2019, 9, 546 13 of 17 (1) Solution accuracy Table 2 shows that for any input joint parameter, whether an integer or a decimal value accurate to nine decimal places, the error between the calculated values (output) and the given values (input) of six joint parameters of the slave manipulator is within 0.00004 degree (or mm), which demonstrates that the proposed approach for the Da Vinci surgical robot inverse kinematics is capable of finding effective solutions with high accuracy.
(2) Solution speed The number of calculation steps of the algorithm/program is one of the influential factors that affect the solution speed.As schematically shown in Figure 3, the number of steps of the inverse kinematics approach proposed in this paper depends mainly on two factors: 1 The number of Newton iterations in the best-case scenario, the numerical solution can be obtained by one iteration when the initial value is perfectly selected.In general, if the given initial value ensures convergence, then the solution of F(x 1 ) = 0 is expected to take approximate 20 iterations; in cases where the convergence fails, the program is designed to iterate at most 50 times and then exit to avoid a dead loop.
2 The step size.when the step size h is large, the iteration times are reduced, thereby shorten the calculation time.However, the selection of initial values will be relatively imprecise, resulting in higher possibility of solution failure due to unreasonable initial values.In cases where the step size is small, the initial values tend to be more precise, resulting in a greater possibility of obtaining a solution.However, the resulting shortcoming is the demand for more calculation steps and longer time.
An improved calculation process is proposed by taking both factors into account: first, start the procedure by following the flow chart shown in Figure 3 with a relatively large step size; second, reduce the step size gradually for more precise initial values if the equation has no solution.This approach boosts the speed of calculation but also reduces the possibility of solution failure.
(3) Singularity influences Our method abandons the use of Jacobian matrix inversion.Therefore, there will be barely no singular solution in the condition of a good initial value from a mathematical perspective.On the other hand, because our algorithm is based on the position increment of the manipulator's joints, Then, the motion joint position trajectory is generated by interpolation based on the initial joint position and the end joint position.The joint position interpolation is not affected by the singularity.

Comparison with Other Methods
Table 4 compared our method with another two typical methods in applicability, complexity of calculation, solution precision, solution speed and singularity influences.Particularly, we choose separates the position and orientation method (name as "separate P&R method" below) to applied on the Da Vinci slave manipulator, and the comparison with the proposed method within the same simulation model is shown in Figure 6.The position curve of "separate P&R method" is following the target with some fluctuation, while the orientation result is far away from the correct solution, which means that the Separate P&R method is not suitable for the slave manipulator that dissatisfied the Pieper principle.Even though increasing the error feedback to compensate for the results may increase the solution precision, the compensation operation will take a large amount of time, which proves the superiority of our proposed method in terms of solution precision and speed.

Conclusions
(1) In this paper, an inverse kinematics mathematical model of the 6-DOF active arm of the Da Vinci surgical robot established by using a coordinate transformation matrix method according to its mechanical motion diagram and working characteristics.By introducing a unitary matrix, the mathematical model on the real plane is transformed to a complex plane, consequently avoiding the relatively complex trigonometric calculations associated with the conventional solution procedure.Following the steps proposed, this approach can apply to other similar robot kinematic problems.
(2) The dialytic elimination method serves to individually eliminate the joint variables and ultimately obtains a high-order nonlinear equation with only one unknown variable of the first joint.Subsequently, the Newton iteration method is introduced to solve the nonlinear equation on the basis of selecting multiple initial values reasonably in the feasible region.The test shows that carefully changing the step size and selecting initial values in the feasible region for the iteration solution of the nonlinear equation will significantly reduce the possibility of solution failure.
(3) In theory, the accuracy of the approach can reach an error of < 0.00004 degree (or mm) and achieve a solution time of 0.5 s (Intel (R) Core i5-4460 CPU, clocked at 3.2 GHz (memory: 4 GB)).

Conclusions
(1) In this paper, an inverse kinematics mathematical model of the 6-DOF active arm of the Da Vinci surgical robot established by using a coordinate transformation matrix method according to its mechanical motion diagram and working characteristics.By introducing a unitary matrix, the mathematical model on the real plane is transformed to a complex plane, consequently avoiding the relatively complex trigonometric calculations associated with the conventional solution procedure.Following the steps proposed, this approach can apply to other similar robot kinematic problems.
(2) The dialytic elimination method serves to individually eliminate the joint variables and ultimately obtains a high-order nonlinear equation with only one unknown variable of the first joint.Subsequently, the Newton iteration method is introduced to solve the nonlinear equation on the basis of selecting multiple initial values reasonably in the feasible region.The test shows that carefully changing the step size and selecting initial values in the feasible region for the iteration solution of the nonlinear equation will significantly reduce the possibility of solution failure.
(3) In theory, the accuracy of the approach can reach an error of < 0.00004 degree (or mm) and achieve a solution time of 0.5 s (Intel (R) Core i5-4460 CPU, clocked at 3.2 GHz (memory: 4 GB)).Running the procedure on better hardware and software (based on the "R12CCPU-V" MELSEC iQ-R series C language controller from Mitsubishi Electric and VxWorks 6.9 operating system) decreases the solution time to 20 ms, which fully satisfies the real-time performance requirement of surgical robots.
(4) A method that combines dialytic elimination and the Newton iteration method is applied to solve the inverse kinematics problem of the slave manipulators of the master-slave surgical robot.The proposed approach achieves higher accuracy than other solutions based on position and orientation separation while the real-time performance is satisfied.Moreover, in contrast to other numerical solutions, the solution method is based on the position of the joint and is therefore barely not affected by any singularity.Further, the application of our approach removes the restriction of the design of the master manipulator configuration.

Figure 1 .
Figure 1.Structure schematic of the Da Vinci patient trolley.(a) Composition of the patient trolley, including four arms (first arm, second arm, third arm and fourth arm) and a frame.(b) Structure schematic of the first arm (slave manipulator), including the passive arm (dashed rectangle area) and active arm (dashed ellipse area), and point H, which is the RCM.(c) Equivalent diagram of the parallelogram mechanism (yellow part).

Figure 1 .
Figure 1.Structure schematic of the Da Vinci patient trolley.(a) Composition of the patient trolley, including four arms (first arm, second arm, third arm and fourth arm) and a frame.(b) Structure schematic of the first arm (slave manipulator), including the passive arm (dashed rectangle area) and active arm (dashed ellipse area), and point H, which is the RCM.(c) Equivalent diagram of the parallelogram mechanism (yellow part).

Figure 2 .
Figure 2. Structure diagram of the active arm of the slave manipulator.To more clearly, we draw H and I as H, H1, H2 and I1, I2, I3 respectively.1-coordinate frame fixed on part 5; 2-coordinate frame fixed on part 8; 3-coordinate frame fixed on part 9; 4-coordinate frame fixed on part 9; 5-coordinate frame fixed on part 10; 6-coordinate frame fixed on part 11.

11 Figure 2 .
Figure 2. Structure diagram of the active arm of the slave manipulator.To more clearly, we draw H and I as H, H 1 , H 2 and I 1 , I 2 , I 3 respectively.1-coordinate frame fixed on part 5; 2-coordinate frame fixed on part 8; 3-coordinate frame fixed on part 9; 4-coordinate frame fixed on part 9; 5-coordinate frame fixed on part 10; 6-coordinate frame fixed on part 11.

Figure 3 .Figure 3 .
Figure 3. Inverse kinematics solution process of the Da Vinci slave manipulator.

Figure 4 .
Figure 4. Range of motion of the slave manipulator.

Figure 4 .
Figure 4. Range of motion of the slave manipulator.

Figure 5 .
Figure 5. Simulation results: (a) simulation in Adams, (b) end position comparison between target and our result, (c) the end orientation (Euler angles) comparison between target and our result.

Figure 5 .
Figure 5. Simulation results: (a) simulation in Adams, (b) end position comparison between target and our result, (c) the end orientation (Euler angles) comparison between target and our result.

Figure 6 .
Figure 6.The endpoint comparison between results of our proposed method and the separate P&R method.(a) The position result comparison; (b) the orientation result comparison.

Figure 6 .
Figure 6.The endpoint comparison between results of our proposed method and the separate P&R method.(a) The position result comparison; (b) the orientation result comparison.

Table 1 .
Structure parameters of the Da Vinci slave manipulator.

Table 2 .
Inverse kinematics solution of the Da Vinci slave manipulator.

Table 2 .
Inverse kinematics solution of the Da Vinci slave manipulator.

Table 3 .
Inverse kinematics solution of the Da Vinci slave manipulator (based on better hardware and software).

Table 4 .
Comparison between the proposed method and others.