Motion Planning for Serial Mechanisms Based on Precurved Flexible Beams

The field of Robot-Assisted Surgery is of great interest. Such robots can improve procedures leading to quicker recovery, smaller scars, less blood loss, and less pain. In this study, we present the motion planning scheme of a flexible mechanism whose motion depends on the insertion of precurved hyper elastic beams into an external multilumen tube that bends according to its geometric and elastic properties. We first define the robot’s shape according to the inserted beam’s known properties. We present the physical model of this system, explore how contact with obstacles affects the robot’s final shape and calculate the forces that it exerts on the tissues on which it leans. We plan the beams insertion order that allows reaching the goals without colliding with obstacles. Finally, we verify our calculations and assumptions on a real-world system. This technology can be used in many robotic fields such as Minimally Invasive Surgeries and Natural Orifice Transluminal Endoscopic Surgeries.


I. INTRODUCTION
Generally, in robotics, mechanisms can be divided into two different types: Parallel manipulators (such as the Stewart platforms [1] and Caster Wheeled Omnidirectional Robots [2]) and serial robots. Serial robots constitute an open kinematic chain with each of the chain joints controlled by the control unit. [3] These serial mechanisms can also be separated into two different types: Rigid mechanisms -mechanisms whose nodes have a fixed geometry and their relative position change depends on actuator movement.
Flexible (compliant) mechanisms -whose shapes are not fixed, and the distance between two points on the same link may change due to external forces [4]. These mechanisms have mainly soft bodies [5] and can move via many different actuation methods [6], [7] including shape memory alloys [8], [9] and polymers [10], [11], motorized tendons [12], electroactive polymers [13], [14], self-healing polymers [15] ferromagnetic elastomers [16], [17], biohybrid [18] and fluidic actuation [19], [20]. Here we present a mechanism that can be categorized as between the two types that we described The associate editor coordinating the review of this manuscript and approving it for publication was Zheng Chen .
above. On the one hand, it is a flexible mechanism whose shape depends on the environment in which it is located [21]. On the other hand, the angle between each of its neighboring segments can be controlled similarly to the degrees of freedom of a serial robot. [22], [23] Actuating serial mechanisms generally involve with a motion planning algorithm which calculates the mechanism's desired shape at each time-step in order to avoid obstacles. The common algorithms used for finding a path in the mechanism's configuration space are Potential Field [24], Probabilistic Road-Map [25], Rapidly-exploring Random Tree [26], [27], [28] A* [29] and Neural Network motion planners [30] (see also [31] which presents the usage of neural networks in surgical robot teleoperations and [32] which presents a breathing pattern monitoring during daily activities).
There are many uses of serial mechanisms in the world of medicine [3], [33]. Some of them have a possibility of actuation [34], and their control methods are similar to the methods used for serial robot control. As for the others -such as endoscopes and catheters, their original shape is not determined by the operator. They align with the shape of the environment according to the external forces such as the walls of the tissues into which they are inserted. Minimally Invasive Surgery FIGURE 1. An example of the flexible mechanism using two precurved beams (red and blue). Reaching the right upper lobe requires a complex mechanical maneuver. Most systems today rely on additional Endobronchial Tubes, since leaning on the lung anatomy may result in damaging the lung tissue. Inserting precurved beams into the bronchoscope's working channel assists such a complex maneuver since unlike bronchoscopes, which possess a single degree of freedom at the tip, it enables curvature control along the flexible tube 1 .
(MIS) offers exceptional benefits compared to conventional open procedures [35]. One branch of MIS is Natural Orifice Transluminal Endoscopic Surgery (NOTES) [33], [36], different from laparoscopic surgeries, since the endoscopic surgical instruments are guided through a natural opening (mouth, anus, etc.) to the areas where surgeries are performed. The field of NOTES requires modern techniques to meet the size and force limitations inherent in this type of minimally invasive surgery (as shown in the example in Fig. 1). Using compliant mechanisms meets these requirements.

A. COMPLIANT MECHANISM PROPERTIES
Compliant mechanism [4] advantages fall into two categories: increased performance (increased accuracy and reliability; reduced weight, wear, and maintenance) and reduction of costs (number of parts and assembly time reduction, simplified production processes). A challenge of compliant mechanisms is to allow deflections that are large enough to fulfill their purpose while maintaining stresses below an allowable maximum stress. 1 Generally, compliant mechanism members go through large deflections, which introduce geometric non-linearities, requiring special considerations in deriving methods for their analysis while small-deflection analysis relies on the assumptions made to solve the Bernoulli-Euler [37] equation. In that case, the slope is assumed to be small, and the curvature is approximated by the second derivative of the deflection. In large deflections, the slopes are not small, and this assumption is not valid. That equation can be solved by the elliptic-integral method as described in Larry Howell's book [4], but the derivations, in that case, are complicated. Nonlinear finite element methods are numerical alternatives to elliptic-integral solutions. The chain algorithm [38] uses the same theory as standard finite element methods but employs a different technique to combine and solve the equations obtained, making it computationally more efficient in many applications.

B. REDUNDANCY IN ROBOTIC SYSTEMS
A hyper-redundant robot [39], [40] has a large (or infinite) number of degrees of freedom. Such robots are useful for operation in highly constrained environments. In this hyperredundant case, there is an infinite amount of solutions. To overcome this, and find the best one, there are many different methods. For example, the pseudo-inverse Jacobian method [41], [42] is used to compute the joint velocities with minimum magnitudes that achieve the control objective (See also [43] where the actuation power is considered).

C. RELATED WORK
In the literature, mechanisms based on the approach presented in this paper are known as Concentric-Tube Robots (see a review in [44]). The researchers in [45] demonstrated the potential of such technology and presented the design principles and the general kinematic model. In [46], the researchers presented a learned model that can estimate the entire shape of a concentric tube robot. The learned model was based on a deep neural network that was trained using a mixture of simulated and physical data. The model was trained on cases where the robot was operating only in free space. The researchers in [47] presented a continuum tubular robot, constructed by telescoping pre-curved elastic tubes, which are capable of balancing the force application and steerability during minimally invasive surgeries. In their paper, a sampling-based motion planning method was proposed based on RRT algorithm. The proposed motion planner maneuvered the robot roughly along the central axis of the statistical humerus atlas in an approximate follow-the-leader manner. The motion planning algorithm was calculated in the 6D space of the actuators (3PR serial mechanism) and the tubes' curvature was assumed to be fixed. The researchers in [48] also calculated the kinematic model of a 3PR telescoping precurved mechanism. Their experimental results illustrate the importance of including torsional effects in order to accurately calculate the mechanism's shape. The researchers in [49] suggested a continuum tubular robot for an active cannula. They also assumed a telescoping tubular robot having constant curvatures and avoided obstacles by calculating the mechanism's shape using a penalty method. Another telescoping pre-curved elastic tubes was presented in [50] where the researchers presented a robot constructed from four telescoping tubes, each having a constant curvature and a motion planning scheme for lung biopsy. In [48], the researchers presented a telescoping tube mechanism with a general measure method for how stable a given robot configuration is.

D. CONTRIBUTION AND PAPER ORGANISATION
In the literature, concentric-tube Robots refer to a series of telescoping tubes having a constant curvature. In this paper, we present a kinematic analysis of a serial mechanism based on parallel insertion of precurved-flexible beams. The beams are inserted into a dedicated lumen, so, due to the mechanical simplicity, multiple precurved beams with the same length may be used. In our approach, there is (theoretically) no limit to the number of beams that are inserted one parallel to the other. So, given the beams insertion sequence, the variable beams curvature is calculated, such that the robot's endpoint will reach the set of desired target points. We then solve the motion between the points using A* and avoid obstacle collision. If one desires a low number of precurved beams (e.g. for reducing the mechanism's thickness), it will result in less maneuverability. Nevertheless, we show how to calculate the force exerted on an objects that the mechanism leans on while assuming that the beams are flexible. The force value can be taken as a cost function and be minimized in the motion planning procedure. Moreover, unlike the telescoping tubes design, during the mechanism's actuation, beams may be entirely removed, so it is possible to plan different intricate paths by alternating the removal and insertion of a large number of precurved beams. This paper is organized as follows: Section II presents the physical and mathematical model and how the robot's shape is affected as a result of the beams inserted. In section IV-A, we describe how to find the shape of the beams to meet all the constraints and requirements. Section IV deals with the use of such mechanisms in an environment with obstacles. We show how the robot bends, what forces the robot exerts, and how the target is reached while avoiding contact with the obstacle. We present the results of the experiments in section V and compare them with the assumptions and calculations from the previous sections. We conclude and present our plans for further work in section VI.

II. ROBOT KINEMATICS
The pseudo-rigid-body model (PRBM) [51], [52] is a method that simplifies compliant mechanism analysis that undergoes large deflections by modeling them with elements common to traditional mechanisms. We use this method in our kinematic calculations. Though our analysis may be used also in a 3D workspace, in this paper we shall focus on the planar case.

A. ROBOT DEFINITION
Each beam is defined as a vector of relative angles connecting the segments in the PRBM. We assume each two neighboring segments are connected by a torsion spring. The spring's constant (as considered below) is calculated by the geometric and elastic properties of the inserted beam. Each robot has a length L that is divided into N segments such that the length of each segment is dL = L N . The relative angle between each pair of segments is θ n . We define the maximal number of beams that are inserted by M . We unite all beam angle column vectors as an N ×M matrix [θ] such that θ n,m represents n angle in the m beam. Beam number 1 defines the outer tube into which the M beams are inserted. The absolute angle n,m is calculated according to the following expression: such that 1,m is the absolute angle of the proximal first segment in the m th beam.

B. CALCULATING THE ROBOT's SHAPE
The final shape of the robot is directly affected by the properties (shape, depth) of the beams inserted inside. Every segment is specified by its relative angle θ n,m , modulus of elasticity E n,m , and moment of inertia I n,m . We define the matrix EI such that EI n,m = E n,m I n,m . In addition, we define the insertion depths of each flexible beam by D m , calculated in segment length unit. The angle between two neighboring segments depends on all the segments of the beams that are at that same depth.
For the purpose of this calculation, we will define the matrix [θ * ] such that the column θ * :,m is a column vector of the angles relative to the outer tube proximal end when the m th beam is inserted D m segments into the outer tube: And on the same principle EI * m vector is also calculated: Based on the beam deflection bending equation [37], the torsion moment M is proportional to the bending angle α: VOLUME 10, 2022 Since the system is in equilibrium, the sum of the torques of the system is equal to 0: θ * n,j represents the change between the final angle θ n,j of the segment and its initial angle θ * n,j in the state of zero energy of the same beam. In fact this is the change in the angle of the same segment in the beam as a result of its bending.
Note that the final angle of the robot is uniform for all the inserted beams because they are coupled. Therefore, the resultant n angle of the entire mechanism is represented by the following equation:

III. GRADIENT NULL-SPACE METHOD FOR SHAPE CALCULATION
To find the optimal shape of the robot, we use the gradient projection on the Null-Space method. The depths of the inserted beams and the desired distal segment position are defined as kinematic constraints for this system.
After defining these constraints, we consider the gradient that will cause the system to converge to meet this constraint. Each calculated gradient vector is projected on the Null-Space of the previous gradients. Thus, we get a solution that meets the requirements according to the system state definition.

A. POSITION ANALYTICAL GRADIENT
Our calculations are made in the mechanism's configuration space C. A configuration c ∈ C is defined by the M · N × 1 column vector: We define the point (x 0 , y 0 ) as one that we desire the mechanism's distal segment to reach. To converge to the desired goal position for the given insertion beam depths we shall calculate the gradient ∇f according to the angles of the inserted beams.
Given a configuration c, one may calculate the distal end position using the forward kinematics: by calculating the gradient: Note that here we set ∂f ∂D i = 0, i = 1, 2, . . . , M since the change in depth will be used in the motion planning task as will be presented later on.
We move in the configuration space by: in order to decrease the distance between the end unit position(x, y) and its final desired position(x 0 , y 0 ). Here k and ε represent the time step and the small step size in C respectively. The example shown in Figure 5 shows the use of the shape calculating algorithm for given constraints. The constraints here are the position of the end unit at the point [0.1, −0.1] while the depths of the two inserted beams (red and yellow) are maximum (N=100).

B. NULL SPACE
After reaching the first goal we would like to maintain this solution and still be able to reach other points by changing some beam insertion depths such that each goal point will be reached by a different set D j , j = [1, M ]. In other words, we found a point c ∈ C that solves f 0 = 0. For a given depths A null space is defined as the linear subspace of the domain of the map which is mapped to the zero vector. This subspace will be used for the gradient vector calculated above.We start by calculating: A given gradient vector has M ·(N +1) elements, and therefore the null space matrix for a single constraint should be with M · (N + 1) rows and M · (N + 1) − 1 columns, and any additional constraint will lower by one system's degrees of freedom. In practice, many elements of the gradient vector are zeros; for example, the angles of non-inserted segments and therefore our system's degrees of freedom number is significantly smaller. Therefore, we have to define in advance a small number of constraints. So, in a given configuration c, motion in the subset K ⊂ C will maintain f 0 , f 1 , . . . , f i−1 . In order to solve f i = 0 we project ∇f i on K [53]. The projection will be made as follows: Then we proceed similarly to Eq.11 by We move in C by repeating Eq.14 until convergence (See Algorithm III-B). Theoretically, after converging to the desired point, the size of the gradient will be equal to 0. Therefore, when we project the next destination gradient on the previous gradient's null space the contact at this point will  Calculate θ 1:N , x N ,y N using equations [6,9] Figure 5 demonstrates the use of a shape calculating algorithm for 4 given constraints. The constraints here are the position of the end unit at the point while the yellow beam is removed for depth of 100 to 0 in four steps.

IV. MOTION IN AN AREA WITH OBSTACLES
After meeting the position requirements under the insertion depth constraints, we analyze the motion of this system in an area that includes obstacles. When passing through an obstacle area, we must consider the following phenomena resulting from the contact of our robot with the obstacle: 1. Robot's shape calculation at the existing configuration based on reliance on the existing obstacles; 2. Consideration of the direction from which the robot came to the obstacle (hysteresis); 3. Calculation of the forces acting on the robot; and 4. Motion planning calculation in the Depth Space D to VOLUME 10, 2022 avoid contact with the obstacle or alternatively reducing the applied force at the robot-obstacle contact point.

A. SHAPE DEFINITION
Unlike rigid robots, for flexible mechanisms, defining the shape of the robot that touches the obstacle is more complicated. First of all, it is important to note that for the same beam shape and insertion depths the system might have several different solutions due to external forces. Therefore the transformation is not effective. In contrast, as long as the robot does not touch an obstacle, the data of the inserted beam shapes together with the insertion depths are sufficient to define the system state. In Figure 7, the shape of the new beam can be directly affected by the direction from which it comes to the obstacle. And since there is a dependence on the path history, this system has hysteresis. There are cases where the start and end states are defined such that the robot does not touch the obstacle, but during the motion between the states, a contact may occur with the obstacle, resulting in a local minima.

B. FORCE CALCULATION
The bending energy in a torsion spring is defined as follows: where ϕ 0 is the zero-energy angle and κ is the spring's constant as defined in Eq.4. So here κ equals to: Since we use the PRBM technique to model the beam as a group of segments connected by torsion springs, we can calculate the stored energy of the entire mechanism as follows: Our first method of dealing with obstacles is to use leaning on those obstacles, to bring the end unit to the desired location. Assume a set obstacle located at During the motion in C in order to reach (x 0 , y 0 ) as described in Eq.11, the mechanism may touch some obstacles. So, as we change the beams' angles θ n,m the mechanism will not reach θ and some θ will occur due to the leaning on the obstacles. To solve this problem, we calculate the robot's new shape under the obstacle constraint. When an intersection with an obstacle occurs we calculate the distance from the obstacle and the n th joint along the mechanism that touches the obstacle by: where x n , y n are calculated in Eq.9. By calculating the gradient of the mechanism's energy ∇U (c) (Eq.16) we may project ∇U (c) on Null(∇f o,i ) and move in C similarly to Eq.14 in order to calculate the mechanism's shape under the obstacle constraints. We continue this until the projection of ∇U (c) on Null(∇f o,i ) vanishes. This scheme allows us to move the mechanism's distal point towards the goal point under the obstacle constraints. In Figure 6 we present how to search for the shape of the robot so that it reaches its destination. In this example, the motion is in an area with obstacles, and the final solution includes contact with an obstacle. A contact between the robot and an obstacle may result in a bending of the mechanism and thus reaction forces will act on the obstacle's wall. Obviously, for medical purposes, there are standards and requirements regarding the forces acting on different tissues when inserting surgical instruments,and thus it is important to calculate these forces. According to Eq.16, we know the energy stored in the mechanism. In this case, it is possible to calculate the force holding the robot in this equilibrium state. In order to calculate the force acting on an obstacle we eliminate the obstacle and move in C using small steps in the direction of −∇U (c) decreasing the energy. During this motion we follow x n , y n path and the energy change along it. We reach equilibrium after some k steps. The amplitude of the force acting on the obstacle is calculated by: where U k and δ k are the energy change at the j th step and the n th joint travel at the j th step respectively. The force direction is tangent to the n th joint route for k = 0.

C. MOTION PLANNING IN THE DEPTH SPACE
In section IV-A we calculated the beam shape under the depth constraints so that the mechanism's distal joint will reach a set of desired points. Obviously, in an obstacle-free environment, changing the beam depth between the known values will bring the mechanism to the desired configurations. We shall now describe how to move between these points in the case of an obstacle environment. The motion planning is calculated in the M -dimensional Depth Space D such that each point d ∈ D represents the insertion beam depth. Each axis value is limited to an integer number at a range of d i = [0 : N ], i = [1 : M ] As mentioned earlier, when touching an obstacle, the robot will bend and its shape will change. Therefore, its distal joint position will also change. In this case, the conversion from the configuration space to the workspace is not effective. The motion planning task may include obstacle interaction, yet, this is beyond the scope of the paper. Here, we shall solve the case where the robot does not intersect with the obstacles during its motion toward the goal configuration (see [5] and [54] for more examples of motion planning for elastic mechanisms). We can formulate the motion planning problem as follows: given an initial and final beam depth d init and d final , find a path in D such that (f obs , i) > 0, ∀i = [1, N obs ] (see Eq.17).
Scanning the whole configuration space and calculating all different states of the robot and checking if any point along the robot touches an obstacle is complex in terms of time and memory (O(N M )). Such a scan was made here for visualization purposes; obviously one may calculate Eq.17 during the motion in D.
We implemented known search algorithms to find the best insertion order: A Greedy motion planner and ''A*'' [55], [56]. Since our goal is to find the path quickly, the search stops when it finds a solution. By reducing the search resolution (i.e. increasing the step size towards a destination), we get a solution in a significantly shorter time. We chose an appropriate step size to ensure convergence on the one hand and to avoid skipping over the obstacle, on the other.
In the example shown in Figure 8, we performed a scan of the entire configuration space for three inserted beams, with each beam divided into 100 segments (N = 100). In general, scanning the entire depth space results in calculating N M configurations (1M configurations in the case of N = 100 and M = 3). To reduce the number of calculations, we increased the step sizes as presented in Figure 9. The tested nodes amount and the total path length of different step sizes are presented in Table 1.

V. EXPERIMENTAL RESULTS AND DISCUSSION
We conducted both real-world and simulation experiments to verify the performance of our system. In this section, we compare the real world robot's behavior and limitations to our programmed simulations. To verify the assumptions that we base our calculations on, we built an experimental system (as shown in Figure 10). Our system consists of a flat perforated base to which the tube can be connected and VOLUME 10, 2022  a camera that captures the flexible beam insertion and the mechanism's final shape.
In this section, we will examine our assumptions from 3 different perspectives: A. THE SHAPE OF THE ROBOT As we described in the section II, the robot's shape is defined according to a PRBM. The relative angle of each segment is affected, according to Equation 6, by the inserted beam segment's relative angle, the flexibility properties (inertia and elastic modulus), and the insertion depth. After tuning all simulator settings, the experimental results were very close to the simulation results.
The results are shown in Figure 11. For a full view of the insertion of the beam, see [57].

B. BENDING AS A RESULT OF A COLLISION WITH AN OBSTACLE
As we described in subsection IV-A, the robot's beams bend due to contact with an obstacle. The robot's final shape is calculated by converging to a minimum energy solution while maintaining the distance constraint from the obstacle's center. In the real-world experiment that is presented in Figure 12, we added a 5cm diameter obstacle and checked the bending of the beam as a result of contact with this obstacle. The simulation result and the final robot's shape were similar with some inaccuracies due to discontinuity in the outer tube shape and tolerance of the flexible beams.

C. CALCULATION OF THE FORCES ACTING ON THE ROBOT
In section IV-B, we presented a method for calculating the forces acting on the beam. This calculation is based on energy method principles. Since we modeled the original beam as a PRBM, the bending of each segment is directly affected by the torque acting on each torsion spring. According to this principle, we simulated the beam deformation as a result of applied force.  To validate this assumption, we loaded different points along the robot with different loads, up to 40g, as exemplified in Figure 13. After a comparison with the simulation results, it is evident that the bending of the beam is as expected with position error of the most distal segment at about 10% of the robot's length.

D. DISCUSSION
We performed several experiments as described above. The purpose of these experiments was to validate the assumptions on which we based the kinematics and motion planning calculations. We found that there was indeed a match between the simulation results and the real-world experiments. Yet, this required calibrating the simulator settings to get results that match the performance. This is more complicated and should take into account the following: Along the real outer tube there is a pattern that holds the inserted beams. During large bends, collisions between these supports can be seen. To avoid them, we have to produce thinner supports or consider their collision when calculating the tubes' bending. In addition, the manufacturing technology of the outer tube and beams is of great importance. Here, we used an FDM 3D printer with Polyethylene Terephthalate Glycol (PETG) which affects the accuracy of the final model. Even using 100% infill, the air gaps between the model walls affect the modulus of elasticity of the model, so using FDM will result in some inaccuracies between the simulation and the real model. Moreover, the resolution and tolerances of FDM 3D printers are limited. The mechanical design may be improved by using hyper-elastic materials such as Nitinol which will result in better performance. In addition, the beams' shape produced from the kinematic calculation are not suitable for PETG beams (e.g., the yellow beam in Figure 5) and a maximal curvature constraint should be implemented in the shape search algorithm, yet, this is beyond of the scope of this paper.
In the motion planning task we obtained that even for increasing the step size 20 times (one-fifth of the beams' maximal depth), we still converged to a solution. Note that there is great importance to the robot's geometry and the obstacles' position on the plane when choosing an appropriate step size; i.e., an obstacle close to the insertion point occupies a larger volume in D thus a larger step-size is preferred since increasing the step size can significantly shorten the number of calculations in the trajectory search in the Depth Space D as shown in Table 1.
The disadvantages of increasing the step size are: 1) The found routes are not always the most optimal. For example, for a step size = 10, the path is 290 steps, while for step size = 1, we reached the destination after 269. 2) With low search resolution, i.e. large steps in D, the solution is not always found. As a result, we will have to increase the search resolution and compensate for higher search time and complexity. 3) There is a presumption that there is no obstacle between 2 points in the trajectory at this resolution. If, as we move along the route, we discover that there is an obstacle in this range, we will perform a search at a higher resolution up to the next point we have found along the solution route.

VI. CONCLUSION
In this paper, we evaluated a flexible robotic mechanism. The actuation method of this system was based on inserting flexible beams into an external tube, which bends according to the shape and geometry properties of the beams inserted into it. In the first part of the study, we presented the calculation of the robot's shape as a result of inserting beams with given geometries to different depths using the PRBM model. Next, we calculated the shape of the beams to meet a shape constraint. Using Gradient descent and Null Space methods, we extended our solution to meet multiple constraints. In this research, we implemented a solution with only position constraints, but this method can be applied to meet other constraints such as the absolute angle at some joint or force acting on the robot. We learned how to deal with obstacles along the path the robot passes and investigated how the robot would bend when it collides with an obstacle. We examined which forces the robot will exert at the tangential point, and implemented various algorithms to search for the robot's distal end trajectory in the beams' depth space. Finally, we did laboratory experiments to validate our kinematics calculations and the forces that the robot exerts on its environment. Using this method can be effectively integrated with the field of medical robotics. The shape of the beams can be designed according to the patient's biological constraints, and special disposable beams can be produced for that specific surgery. In our future work, we plan to investigate the effect of insertion depth and angle of the entire outer tube on robot performance according to requirements. We will examine how the robot's performance can be changed by controlling its compliance properties; i.e., by changes in the beams' geometry and material properties. We will also try to upgrade the search algorithm performances and implement them on a complete robotic system that we are designing which implements the beam insertion principle. In addition, we intend to produce a system consisting of a uniform outer tube according to the dimensions of the inserted beams so its performance will be more accurate and identical to the theoretical model.