Inverse Kinematics of Concentric Tube Robots in the Presence of Environmental Constraints

Inverse kinematics (IK) of concentric tube continuum robots (CTRs) is associated with two main problems. First, the robot model (e.g., the relationship between the configuration space parameters and the robot end-effector) is not linear. Second, multiple solutions for the IK are available. This paper presents a general approach to solve the IK of CTRs in the presence of constrained environments. It is assumed that the distal tube of the CTR is inserted into a cavity while its proximal end is placed inside a tube resembling the vessel enabling the entry to the organ cavity. The robot-tissue interaction at the beginning of the organ-cavity imposed displacement and force constraints to the IK problem to secure a safe interaction between the robot and tissue. The IK in CTRs has been carried out by treating the problem as an optimization problem. To find the optimized IK of the CTR, the cost function is defined to be the minimization of input force into the body cavity and the occupied area by the robot shaft body. The optimization results show that CTRs can keep the safe force range in interaction with tissue for the specified trajectories of the distal tube. Various simulation scenarios are conducted to validate the approach. Using the IK obtained from the presented approach, the tracking accuracy is achieved as 0.01 mm which is acceptable for the application.


Introduction
Nowadays, continuum robots (CRs) have found widespread applications in medicine, especially in minimally invasive surgeries (MIS). Catheters are an example of CRs that are widely used in MIS. In recent years, comprehensive researches have been done on the design, manufacturing, and development of steerable catheters.
CRs could be modeled using two main approaches: (i) Cosserat rod theory and (ii) constant curvature [1][2][3][4]. The former provides a precise model of CRs using differential equations [5][6][7] while the latter provides a simpler model with less time-cost and still acceptable accuracy [8,9]. Lyons et al. [10] presented a new method of optimization in IK for medical devices to plan configurations with anatomically constrained for reaching specified targets. In this paper, they calculate start position and orientation and a geometric rep-resentation of the physical environment extracted from preprocedure medical images. Forward and inverse kinematics of concentric tube continuum robots (CTCRs) using various geometrical approach are investigated in [11,12], respectively. Anor et al. [13] presented a novel systematic approach to optimize the design of CTCRs for neurosurgical procedures. These procedures require that the robot approach specified target areas while navigating and operating within an anatomically constrained workspace. A particular advantage of this approach is that it identifies the need for either fixed-curvature versus variable-curvature sections. Due to the vast variety of applications in concentric tube robots, the optimization of concentric tube robots has always been alluring for researchers. For example, a new design of CTCR which is well suited to MIS inside small body cavities such as the heart is presented in [14]. This paper presents a generalized pattern search to optimize concentric tube robots while they can reach the target point with minimal curvature and length. Runge et al. [15] used evolutionary algorithms such as genetic algorithm to optimize a soft robot. Bodily et al. [16] used a genetic algorithm to optimize the reachability, dexterity, and manipulability of a multisegment continuum robot. Luo et al. [17] designed a concentric tube manipulator that is appropriate for surgical environments. They presented a new topology of the tubes for the concentric tube robot, which can increase the stable workspace because it allows the usage of larger tube curvatures and/or curve lengths. Davarpanah et al. [18] optimized a concentric tube robot using a genetic algorithm which allows them to reach the specific destination with high accuracy. Lloyd et al. [19] presented a novel model for optimizing a task-specific in millimeter-scale for magnetically actuated soft continuum robots used in medical applications. Finally, most similar to our work, Cheong et al. [20] applied a computational method to find optimal designs of continuum robots while considering reachability constraints.
This paper contributes to the field by finding an optimized inverse kinematic solution for a three-segment CTCR so that for a given position, the robot configuration would occupy minimized space. The proposed algorithm is very beneficial in MIS, where the robot needs to work in a tight space. Moreover, the optimized solution meets a safety interaction criterion at the base of the CR. In practice, when the robot inserts to an organ cavity, its shaft body would remain in the colons or vessels connected to that cavity. So, it is important that at the base of the robot, which is inside a tighter space, the CR does not apply forces to the surrounding tissues that may hurt.
In MIS applications, the robot is ideally expected to perform its mission with occupying the least possible workspace. To address this issue, in this paper, an algorithm is proposed that helps the minimum number of segments of a threesegment CR be involved in achieving a target point. Moreover, the robot-environment contact is modeled in the form of a spring which allows to apply interaction-force/displacement constraints to the base of the robot which assures a safe interaction. Finally, the research is concluded in Section 4.

Forward and Inverse Kinematics
In this section, the structure of the constant and variable length of a three-segment CTCR is described. The modeling assumptions of the CR are summarized as follows.
(i) The bending motion of the robot is planar (ii) The curvature of the robot backbone is supposed to be constant (iii) The curvature of each segment of the robot remains constant, thus, the central backbone is always located within a plane Figure 1 shows a three-segment concentric tube continuum robot. Based on the following assumption, the constant curvature approach is utilized to model the system.
The parameters used to describe the model of a threesegment continuum robot are listed in Table 1.
Equation (1) calculates the length change of each segment.
where L i0 is the length of the i th segment in the primary situation of insertion, and ΔL i is the decrease or increase in the i th segment length which is taskdependent. Then, Eq. (2) shows the total length of the catheter.
2.1. Coordinate Systems. Figure 2 presents the relations between actuator space, configuration space, and task space. The actuator space includes tube lengths, the configuration space covers the robot bending angles and the insertion length, and finally, the task space is relevant to the end effector's position and orientation. The coordinate systems for a single-segment continuum robot (see Figure 3) are described as follows.
(1) Base Coordinate System. The base coordinate system represented by fbg = fx, y, zg (2) End Coordinate System. The coordinate system is located at the end point of the tube (i.e., the intersection of the tube with its nearby tube). The coordinate system is shown by E i = fx i , y i , z i g, i = 1, 2, 3 for a three-segment catheter

Kinematics of Multisegment Robot.
For the slender bar model, the curvature is not a constant and it can be widely used [21,22]. As mentioned previously, the constant curvature approach is used to model the kinematics of the robot ( Figure 3) [23,24]. The general mapping between the task space and the configuration space can be obtained by a homogeneous transformation matrix (Eq. (3)),  Applied Bionics and Biomechanics

R
The radius of curvature in the primary backbone is defined in the bending plate (the plate of The bending angle in the primary backbone is defined in the bending plate (X 1 Z 1 plate) at the point O′. φ ∈ 0 2π ½ The orientation angle of the robot which can rotate in the xy plate φ prox , α prox Rotation and bending angles of the first part of the catheter φ med , α med Rotation and bending angles of the second part of the catheter φ dist , α dist Rotation and bending angles of the third part of the catheter The number of robot segments The rotation matrix from the end disk coordinate system to the base coordinate system 3 Applied Bionics and Biomechanics where R ot represents the orientation of the end effector, p is the end position of the robot [25], c and s stand for cos and sin, respectively. Using the length and radius of the robot curvature shown in Figure 3, the angle α can be calculated using where R, l, and k are the arc length, bending radius, and curvature, respectively. More details on how to obtain Eqs. (3) and (4) could be found in [25,26]. The forward kinematic model of the three-segment CR can be obtained using 2.3. Inverse Kinematic for a Multisegment Robot. Increased degrees of freedom (DOF) bring more redundancy for the robot configuration. As a result, there are no closed-form solutions for the multisegment robot inverse kinematic. Moreover, there would be multiple solutions for the problem of finding the inverse kinematics. To obtain the forward kinematic model, two different cases need to be considered for the bending angle of the distal segment [27] including θ dist < 90°and 90°≤ θ dist ≤ 180°. The position of the central backbone of the catheter in the second and third segments is achieved using where the points P T 0 = ½x 0 y 0 z 0 , P T 1 = ½x 1 y 1 z 1 , P T 2 = ½x 2 y 2 z 2 , and P T 3 = ½x 3 y 3 z 3 are the catheter base coordinate system, the intersection of the first and second segments, the intersection of the second and third segments, and the end-plate of the third segment, respectively. More details can be found in [27]. Vectorsp 2 and p 3 are local positions of the end-effector in the coordinate systems at the base of each tube. Also, the optimized parameter would be the configuration parameters including bending and rotation angles and insertion length of each segment. According to the number of extended segments (e.g., when two or three tubes are inserted out from the overlapped tube), the catheter tip can be denoted by eitherp 2 orp 3 : To form cost function, we first rearrange Eq. (6) as follows Next, the cost functions of the two-segment and threesegment catheter can be defined in the form of where γ is a weighting factor. Figure 4, if you want the robot to be safely interacting with the texture, at first, the texture should be modeled into a spring, then, the amount of the allowed displacement for being in contact with the robot base should be obtained. Allowed ranges of the interaction force vary across surgical tasks. The size and fragility of the tissue are important parameters affecting the allowed force range. In this paper, the following assumptions are made according to the literature [28,29].

Safe Interaction Conditions. According to
(i) Diameter of the main coronary arteries is about 4 mm [30] (ii) The maximum allowed force applicable to the texture is considered as 0.2 N. The texture stiffness, at the entrance of the cavity, is considered to be k = 100 N/ Applied Bionics and Biomechanics m which corresponds to 2 mm displacement of the tissue which is acceptable (Eq. (10)) Δx T equals to the required displacement of P b = ðx, y, zÞ so that the catheter tip would reach to the target point.
To obtain Δx T , first, the shape of catheter corresponding to the desired tip catheter is obtained, and then among the multiple solutions, those solutions which are meeting the safety constraint at the point of P b are accepted.

Results and Discussion
In this section, first, we simulated the kinematic model obtained in the previous section using MATLAB. Figure 5 demonstrates the simulation results for the three-segment catheter. The robot configuration parameters corresponding to the target point of the catheter tip are listed in Table 2. Figure 6 shows the mapping between the position of the robot end-effector and the configuration space variables. At each iteration, the bending and rotation angles change smoothly  Catheter tip Figure 4: The safe interaction with the texture modeled by the spring.
Step-increment equals to 1:8°for α and 3:6°for φ. The number of the chosen sample points is 100. The weighting factor γ is set to 50.

Continuum Robot Workspace.
In this section, the robot workspace for a single, two, and three-segment catheter is obtained and demonstrated. The variation range of the configuration parameters are shown in Table 3.
As illustrated in Figures 7-12, with the increase of the number of robot segments, the thickness of the clouds forming the workspace is increasing, which reflects an enlarged accessible space and increased precision of the threesegment catheter compared to single-or two-segment catheter. Hence, one can conclude that multisegment catheters are more appropriate choices for applications in which the catheter is supposed to operate in confined spaces.

Continuum Robot with Variable Length: Insertion
Optimization. In this section, we consider a continuum robot with variable length. The purpose is to optimize the robot segment lengths while meeting the aforementioned constraints. The schematic of the proposed optimization algorithm is shown in Figure 13. In this diagram, first, concentric tube robot receives the desired point and then calculates the distance between the robot's tip and the desired point and increases the tube's length based on the calculations. If the desired point is in the range of tube 1, only segment 1 would be involved to help the endeffector reach the target point. Otherwise, the second and the third segments would be inserted to the workspace.

Optimization Example: A Catheter with Two Segments
(No Insertion Is Allowed). We consider a two-segment cathe-ter of 120 mm length (60 mm for each segment). The target point is set to P T ½−47:98 − 33:92 33:90. Using the proposed algorithm, as it has been shown in Figures 14 and 15,    Applied Bionics and Biomechanics after 2000 iterations, the optimal parameters of the robot including the bending and rotation angle of each segment can be observed in Tables 4 and 5. According to Figure 14, the amount of the cost function after about 40 first iterations is approximately equal J = 8:03.
We also consider the hypothetical target point in the coordinate system P T = ½40:85 77:24 26:97 of for the optimization of the three-segment catheter configuration space with a general length of 120 mm and a separate length of 40 mm for each segment.
As it can also be seen in Figure 15, the amount of the cost function, after about 60 first iterations, equals J = 6:59.
The simulation results show that the catheter accuracy to approach the point in question is too much, also, a considerable amount of optimization, in the first several iterations, shows itself; as a result, it will be possible to reduce the iteration limit when the velocity of the process seems to be important to us.

Optimization Example: A Catheter with Two and Three
Segments (Insertion Is Allowed). As it was mentioned, in the variable-length catheter, considering the target point, first, the proximal segment length increases from its primary length considered to be zero to its final length, and this same trend for other links reports in order to approach the target point.
The three-segment catheter will have the same length for each segment which is 40 mm has been used, so as to simulate how to access the variable-length catheter.
We  Table 6. According to these optimal parameters, the order of error is 0:01 mm for two segment catheter which is small in comparison to three segment catheter that has an error greater than 1 mm. Therefore, the variable-length catheter will use two-segment of its inner parts in order to approach the desirable point, and this same result has been shown in the last row of the first column. Also, the optimization amount of the two-segment cost function has been obtained to be the number of J = 1:1032 that is an appropriate amount for tracking the target point.  Table 7 without the safe interaction with the fragile structures like texture. If dB < Δx T , the obtained position and the parameters related to this position are correct, if not, another response is chosen.
According to these optimal parameters, the order of error is 0:01 mm for a two-segment catheter which is small in comparison to three segment catheter that has an error greater than 1 mm. Regarding the error amount, for each segment, the desirable link will be the second link. Also, the amount of the cost function for the two-segment of the variablelength catheter will be equal to 1:4539.
As you can see in Table 8, for the catheter safe interacting with fragile structures like texture, according to the Eqs. (8) and (9), at first, we obtained the safe limitation constraint

Start
Get the target point The distance from tip's catheter is calculated Compare the answers A, B, C and give the optimal answer If L 1 < dist < L 1 + L 2 Compare the answers A, B and give the optimal answer If L 1 + L 2 < dist < L 1 + L 2 + L 3

No Yes No
No A) Find the optimal solution by solving the inverse kinematics code of a single-segment robot B) Find the optimal solution by solving the inverse kinematics code of a two-segment robot C) Find the optimal solution by solving the inverse kinematics code of a three-segment robot A) Find the optimal solution by solving the inverse kinematics code of a two-segment robot B) Find the optimal solution by solving the inverse kinematics code of a three-segment robot A) Find the optimal solution by solving the inverse kinematics code of a three segment robot dist = x 2 + y 2 + z 2 Figure 13: Flow chart for finding an optimal inverse kinematics. 9 Applied Bionics and Biomechanics for interacting with fragile structures like texture. Considering the two mentioned conditions, we take into consideration the maximum amount of the allowed force which is applied to the texture to be 0:2 N, and also the stiffness to be 100, at the entrance of the cavity. According to Eq. (10), the displacement amount at the contact point with the texture will be obtained as ±2 mm. Now, after required iterations from the population in the algorithm, in each of the robot segments, the correct link for approaching the chosen target point and the angle amounts of the catheter are obtained in the case that the distance amount of the robot is less than 2 mm.       10 Applied Bionics and Biomechanics According to the obtained error amount from the hypothetical target point and the tracked point in the threesegment catheter, it can be concluded that the three-segment catheter has an undesirable tracking for approaching the target point, and also according to the obtained points in the table second column, and the little error amount the catheter will use two segments from its inner parts for approaching to the desired point.
Also, the optimized cost function amount of the twosegment catheter is obtained to be 2.8294 which is an appropriate amount for tracking the target point. In the last row of the first column, the allowed displacement amount at the contact point with the texture has been gained which is equal to dB = 1:1641 mm, which is less than the indicated amount of 2 mm. This shows the catheter's safe interaction with the texture.

Conclusion
In this paper, the optimization and kinematic model of the continuum robot target tracking have been surveyed in two states of constant and variable lengths. To do this, the method of the constant curvature was applied, considering the bending of each segment of the robot in the Platform, extracting the forward kinematics for constant and variable lengths, and using inverse kinematics for the robot. This robot's being redundant has caused it not to require the closed-form solution for its inverse kinematic. For this reason, optimization and numerical solving methods have been used for tracking the target by the robot. Adding the possibility of the robot length change, the robot workspace and redundancy are increased, and we can use it to provide more dexterity. As it was done in this research, whereas the length changes of each one of the segments are influenced using the offered algorithm as a variable of the configuration space for approaching the target point, it is possible to use it as a tool for better control of the robot. Also, the robot movement limit will be more appropriate for environments like the vessels. In this paper, the results of the forward kinematics parts confirm the results of the inverse kinematics parts, and vice versa, which indicates the accuracy of the model. Also, for the part of the optimization of the multisegment catheter, the results also indicate the model appropriate accuracy in tracking the giving points for performing endovascular surgery. The average distal end positioning error is less than 1% for each segment of the catheter, which is an acceptable error for most real-time surgeries. In the final part, regarding force imposed onto the texture, and also the stiffness, the allowed distance amount with the texture causes the safe interaction. The implementation of the secure interaction with the texture and the trajectory results indicate the catheter has an accurate application in the medical procedure. Future works focusing on the addition of more constraints, and optimization of the distinguished constraints of the cost function, try to lessen the tracking errors and to improve the stability of the existing catheter. Also, tracking a certain path concerning the safe interaction with the texture for the catheter will be carried out.

Data Availability
The MATLAB code data used to support the findings of this study are available from the corresponding author upon request.