To illustrate how the proposed method works in practice, we used the i ² Snake robot as an example to demonstrate the functionalities of the algorithm. As MOVE uses the Denavit-Hartenberg (DH) convention, this framework would also work on other types of articulated robots.

The detailed forward kinematics of the i ² Snake was previously presented in (33). The i ² Snake is a tendon-driven articulated robot composed of 12 rolling-joints arranged orthogonally. The 12 joints are grouped into 3 sections named proximal, middle and distal and having 4 DOFs each, which are then mechanically coupled into 2 DOFs. The base of the i ² Snake further provides 1 DOF roll motion. The i ² Snake is then attached to a robotic arm providing the insertion motion required to navigate inside the patient. All together, the i ² Snake presented in (33) has 8 controllable DOFs. However, due to the type of joint used (rolling-joint) and its particular rolling motion, the i ² Snake DH table requires 26 joint variables with intrinsic mechanical coupling. The reader can refer to (Berthet-Rayne et al., 2018b; Berthet-Rayne et al., 2018c; Berthet-Rayne et al., 2019) for further details on the i ² Snake design, kinematics and the joint coupling.

In the context of this paper, the i ² Snake kinematics is further augmented with additional 4 DOFs to allow 6 DOFs operation at the base of the robot and fully exploit the robotic arm holder capabilities as shown in Table 1, lines 1 to 6.

During endoscopic surgery, the tip of the endoscope is usually equipped with an imaging system such as a camera or an optic fibre bundle. In traditional endoscopy, 4 DOFs are available to the endoscopist: the roll, the pitch, the yaw, and the insertion, all controlled manually from the back of the endoscope and using the video. Therefore, the same 4 DOFs are also used on the proposed robotic approach. The head of the robot, where the camera is installed, is determined by two 3D points, one at the base p b ⃗ and one at the tip p e ⃗ forming a vector v head ⃗ = p e ⃗ − p b ⃗ . The orientation of the head is controlled by rotating v head ⃗ with the rotation matrix R _head (containing the roll, pitch and yaw angles) around the point p b ⃗ as follow: p e ⃗ = R head v head ⃗ + p b ⃗ (1) The insertion motion Δa will induce a translation of v _head along itself: v head ⃗ t + 1 = Δ a v ̂ head t (2) where [ v ̂ head ] t represents the normalized vector v head ⃗ at time t, and [ v head ⃗ ] t + 1 the translated vector v head ⃗ at time t + 1. This process is represented geometrically in Figure 2. Once the position and orientation of the head known, the next step is the path creation.

The head, more specifically the base of the robot head p b ⃗ , is used to determine the path to be followed by the rest of the body. Every time the insertion is changed by the operator, the vector v ins ⃗ between [ p b ⃗ ] t and [ p b ⃗ ] t + 1 is computed and its corresponding norm d is calculated. If d is equal or greater than the sampling resolution s, a new point is added to the path at index m. This process is depicted in Figure 2 and summarized in the Algorithm 1.

Algorithm 1

Path Creation

Only points that correspond to a change in insertion are saved as the operator might explore the surroundings by changing the head orientation before moving forward toward the site of interest.

A virtual robot is used to identify target points for the real robot along the path. This virtual robot is a modified version of the real robot being controlled. It has the same amount of links and same link length. The only difference is that each revolute joint is modelled as a universal joint with 3 DOFs. This feature ensures that the virtual robot’s link ends can be fitted exactly on the desired path as in typical FTL navigation.

The head link of the virtual robot is known from the previous navigation step. The rest of the link’s position is determined using a backward fitting (from head to base) onto the path. In this section, the first link will refer to the head and the following links will be starting from the head, going towards the base of the robot. The fitting consists of mapping each extremity of each link onto the path. This is done in an iterative process for the n DOFs, with each link’s end being the start of the next one, and is repeated until the base is reached. The head link extremities are already known, so the next step is to fit the remaining link’s end onto the path. Two cases can arise while doing so.

Now that all the intermediary target points are known, the next step consists of finding an inverse kinematics solution for the robot to follow the desired path. Conventional inverse kinematics algorithms consider only the end effector of the robot. The Jacobian approach allows to iteratively find a solution to place the tip of the robot in a desired position and orientation without explicitly considering intermediary joints. The Jacobian is defined as follow (Siciliano et al., 2010): v e = p ̇ e ω e = J q q ̇ (6) where v _e is the end effector velocity, J (6 × n) is the geometric Jacobian matrix of the robot defined in the base frame, n is the robot’s DOFs, p ̇ e is the linear velocity of the tip, and ω _e the angular velocity with J defined as follow (Siciliano et al., 2010): J = J P 1 J P n … J O 1 J O n (7) where J P i ( 3 × 1 ) and J O i ( 3 × 1 ) relate to linear velocities and angular velocities respectively. J P i is calculated iteratively as follow (Siciliano et al., 2010): J P i = z i − 1 for a prismatic joint z i − 1 × p e − p i − 1 for a revolute joint (8) and J O i is calculated iteratively as follow (Siciliano et al., 2010): J O i = 0 for a prismatic joint z i − 1 for a revolute joint (9)

In order to follow a desired path in a serpentine motion, one must consider all the joints of the kinematic chain. To this end, we introduce herewith the concept of a full-body Jacobian matrix J _F . The concept of extending/augmenting the Jacobian of a manipulator to consider additional constraints such as joint limits or obstacles is known in the literature (Sciavicco and Siciliano, 1988; Slotine, 1991). In the context of this paper, the proposed full-body Jacobian is combined with a full-body inverse kinematic algorithm allowing to control all the joints individually and in a controlled way.The objective of the proposed algorithm is to minimize the error between each path point and each link’s end of the robot. As the path points are defined in 3D space, only the Cartesian distance is needed for the intermediate body joints, so only the linear velocity need to be considered. The head of the robot however needs to be controlled in orientation as well, hence only the angular velocity of the robot’s tip need to be computed. As a result, the extended Jacobian J _F  ((3n + 3) × n) stacks the Jacobians of each link as if it was an end effector and is defined as follow. J F = J P 11 J P 1 n J P 21 J P 2 n … J P n 1 J P n n J O 1 J O n (10) where J P i i ( 3 × n ) and J O i ( 3 × n ) relates to linear velocities of each link’s end of the robot calculated as a traditional Jacobian (Siciliano et al., 2010). Entries of the Jacobian which are not defined (first columns of first links) are set to 0 as shown in the Algorithm 3.

Algorithm 3

full-body Jacobian Computation

Finally, the full-body inverse kinematics equation can be expressed as follow: q ̇ = J F † q v f (11) where J F † ( q ) is the Moore-Penrose pseudo-inverse of J _F computed using a singular value decomposition, and v _f is the full-body velocity vector. Eq. 11 can be used to solve for the full-body inverse kinematics by integrating the velocities over time iteratively (Buss, 2009): q t + 1 = q t + α J F † q t e f (12) with α a scalar that can be used to scale the convergence at each iteration, and e _f  ((3n + 3) × 1) is the full-body error between the current robot position and the desired path target points, defined as follow: e f = e P e O (13) with e _P  (3 n × 1) the position error and e _O  (3 × 1) the orientation error of the snake’s head. e _P is calculated as follow: e P = p d − p e q (14) with p _d  (3 n × 1) the vector of desired target points, and p _e  (3 n × 1) the vector of the current state of each link. e _O is computed using the angle and axis approach (Siciliano et al., 2010): e O = β s i n ϑ (15) with β and ϑ representing the angle and axis respectively of the rotation between the current robot orientation and the desired orientation. Eq. 12 is the main equation used to find a local optimal solution that will bring the joints as close as possible to the desired path. This iterative equation is used every time the head of the robot is moved. Therefore, using (12) in real-time would result in a FTL-like motion, where the robot follows the path in the best physically possible way.

However, the concept of MOVE consists of using all the available surrounding space which Eq. 12 cannot fulfill. To allow this extended feature, this paper proposes to add a weighting matrix to the iterative Eq 12. The goal of the weighting matrix is to assign a weight to each link’s end which will determine the degree of importance to bring this specific link close to the path. Higher weights mean that the point must be as close as possible to the path, while lower weights will result in the link’s end being further away. As the robot moves inside the lumen, the weight can be altered in-situ to obtain different behaviours depending on the available space around the links of the robot. In the latter scenario, the weights can be set from various types of sensors, such as tactile, stereo-vision, infrared, ultrasonic, etc. The final weighted iterative equation is shown below: q t + 1 = q t + α J F † q t W e f | e f | (16) with W (e _f ) ((3 n + 3) × (3 n + 3)) the diagonal weighting matrix calculated using the error vector. There is a vast variety of weighting scenario that can be used to calculate W (e _f ) depending on the final application, the sensing method used and whether the signal is discrete or continuous.

One possible approach is to allow each link of the robot to be within a set distance of the target. If the robot is further from this threshold, the robot should converge toward the solution; as the link approach, they should continuously slow down the convergence until the desired threshold is reached. This can be done using the following equation to calculate W (e _f ): W i i = s i g n e f i 1 − w i e − a b | e f i | 3 if  e f i ≠ 0 0 if  e f i = 0 (17) with w _i a scalar allowing to turn weighting on (w _i = 1) or off (w _i = 0), a and b two scalars than can be used to set the desired threshold distance. An example of used values is plotted in Figure 4. The advantage of using Eq. 17 is that the error is almost unaffected while outside the threshold distance as the function will tend to 1 and results in a linear function, and it will quickly and continuously attenuate the error once within the desired threshold distance and as a result stop the convergence of Eq 16. As some joints will be allowed to move more as they are less important to the solution, it is then possible to exploit the null-space of the robot to add a second constraint to the optimization (Liegeois, 1977) such as staying as close as possible to the joint centre position to reduce tendon stress due to excessive joint bending and resulting friction. This can be done as follow: q t + 1 = q t + α J F † q t W e f | e f | + I − J † q t J q t φ (18) with φ the secondary objective function allowing to reduce the tendon stress by keeping the joints as close as possible to their centre position, and is defined as follow (Girard and Maciejewski, 1985): φ = ∇ H  and  H = ∑ i = 1 n η i θ i − θ c i 2 (19) with η _i a scalar acting as a gain with values between [0,1], and θ c i the centre value of joint i. It is important to note that Eq. 18 uses both the full-body Jacobian and the standard Jacobian as defined in Eq. 7.

The final feature of MOVE is to allow conservative retraction motion by using the same path than the one used during the insertion and is discussed in the next Section.

Conservative motion is a critical feature in endoscopic surgery. As the robot navigates inside the lumen, the surgeon uses the vision feedback combined with his/her situation awareness to find the safest path to follow. Therefore, the robot must follow the exact same path during the retraction phase which cannot be insured with traditional tip control. As the path is sampled and saved during the path creation phase, the proposed navigation framework has the intrinsic property of being conservative.

The proposed navigation approach, using all the available space, and trying to find the best fit to the path, can also be exploited to compensate for potential mechanical failures that can arise during surgery. Tendons are ideal for miniaturization and remote force transmission, but are also prone to failures if exposed to excessive forces or friction. Types of failure include, for example,: complete snapping, broken core, bird-caging, kinking, and it is safer to stop the actuation when any type of failure occurs. Typical tendon driven instruments on existing surgical robots are therefore restricted to a limited amount of use to avoid the risk of such scenarios from happening, but such failures are still known to occur (Freschi et al., 2013). Although rigid instruments can often be extracted and replaced quickly, in the case of flexible systems, the loss of one or multiple tendons could potentially prevent the safe extraction of the robot. In the case of the i ² Snake robot, there is a considerable amount of tendons used as each pair of joint is connected to 2 tendons (26 tendons). Considering that a tendon failure can be detected by either monitoring the motor current, or by measuring the tendon tension with sensors, the proposed MOVE framework has the intrinsic capability to compensate for the loss of one or more DOFs. In (33), we introduced a method to handle joint-limits by modifying the corresponding Jacobian columns. This approach can be further extended to also handle mechanical failures and ensure a fail-safe mode allowing the operator to finish the procedure and safely extract the robot. The full-body Jacobian J _F ((3n + 3) × n) presented earlier can be represented as an aggregate of column vectors (Ben-Gharbia et al., 2014): J F = j 1 j 2 … j n (20) Where each column j _i represents the contribution of joint i’s velocity toward the movement of the robot in Cartesian space. In the case where a tendon attached to joint i is found to be faulty, the corresponding column j _i will be set to the column vector zero as follow: j i = 0 ⃗ if tendon  i  is faulty j i otherwise (21)

Using Eq. 21 reflects the loss of one or multiple DOFs in the least square equations. As the column is set to zero, the contribution of that joint is voided and the corresponding joint value θ _i will not be changed anymore. As the MOVE framework tries to minimize the distance from the path while allowing some free space around it, Eq. 11 now considers the lost DOF and finds the best fit to the desired path. As the robot is redundant, the remaining operational joints will therefore move to compensate for the faulty one(s).

The results of the implementation of Eq. 12, which is a basic full-body inverse kinematics solver, are shown in Figures 5A, 5B, 6A. In this case, the i ² Snake is fitted to the path without weights or tendon-stress reduction. It can be seen in Figures 5A, B that the virtual fitting (pure FTL navigation) matches the head’s path perfectly and that the robot’s link are fitted as close as mechanically feasible to the path to minimize the overall error norm.

As the robot’s joints are mechanically constrained, and therefore are not free to move in all directions, it can be seen that several joints need to be actuated to match the desired i ² Snake’s head position and orientation. As a result, there is a significant difference between the expected and real head position, which is not a desired behaviour for surgical teleoperation as the robot’s head should be exactly where the surgeon specified during teleoperation.

The basic solver still allows to follow complex 3D motion as depicted in Figure 7 where the i ² Snake is fitted onto a coil-like trajectory with limited error.

Although the basic solver does not guarantee that the robot’s head is exactly on target, its performance was also evaluated during a simulated navigation down the throat as shown in Figure 6A. It can be seen that the robot is able to navigate down the oesophagus in a FTL-like fashion. This basic-solver results show the potential of the MOVE framework as the features introduced inSection 2 (body weights, error tolerance, path weights, stress reduction, and mechanical fault tolerance) will be added.

The results of the weighted solver presented in Eq. 16 are shown in Figures 5C, D. In this example, the head and the base of the i ² Snake were given the highest weight while all the other joints were assigned the lowest weight. This results in the control of the head being more accurate than without any weights. This can be seen in Figures 5C, D as the head follows the desired path exactly at the cost of the other joints moving to compensate for the desired configuration. This behaviour is still unsafe for clinical use as the joint motion of low-weighted joints is still not controlled and can result in undesired configuration.

Introducing a controlled error tolerance is key to safe navigation inside a lumen. This can be done by implementing Eq. 16 into the solver. The results with different error tolerances are shown in Figure 6B (large tolerance) and in Figure 6C (small tolerance). In this case, the error tolerance is manually assigned to the individual i ² Snake’s joints, but in practice this information would be sensor dependent and would reflect the surrounding anatomy’s shape.

In order to further demonstrate the key capabilities of the MOVE framework, in the rest of this paper, the i ² Snake robot was extended by doubling its length and amount of joints. This can be done by adding the entries 7 to 30 at the end of the DH Table 1. This results in a new hyper-redundant extended i ² Snake robot with 54 joint variables as shown in Figure 8. The longer i ² Snake robot can reach further down to the stomach. Figure 8 shows an example of insertion.

The concept of weights can be further extended so the weights are not assigned to the robot directly but rather to the path itself. As the i ² Snake is navigating and the path is created, path weights can be assigned to the path points using sensor information. As the robot navigates, these path weights can be dynamically assigned to the joints passing the corresponding path points. This method has the advantage that it creates a map of large and narrow passageway and can be integrated with sensing modalities such a stereovision or tactile sensing. The result is shown in Figure 9, where both high path weights (right side) and no path weights (left side) are assigned to two-halves of the i ² Snake’s body during navigation. The advantage of using sensor information is that the weights would be continuous based on the sensor data and would result in smoother trajectory profiles.

The results of the tendon stress reduction are depicted in Figure 10. The tendon stress reduction was evaluated during a retroflex motion with a large error tolerance. Having a large error tolerance allows for better tendon stress reduction as more space can be used to move the intermediary joints. Figures 10A, B have no tendon stress reduction, resulting in some joints having sharp bending angles. Figures 10C, D show the exact same trajectory but with tendon stress reduction. The resulting robot configuration spreads the joints angle more evenly and has less sharp bending angles. A rendering of the robot in the same two configuration is shown in Figure 10 which shows that for the same trajectory, configuration a) reached joint limit, while configuration c) can still move further.

The results of fault tolerance are shown in Figure 11. In this Figure, a) and g) depict a normal ‘healthy’ robot that can follow the trajectory with a maximum error of 2 mm. The detailed trajectory error is plotted in d) and shows that most links can be fitted with a sub-mm accuracy. b) Shows a faulty i ² Snake with a damaged 4^th section with the faulty joints in a close to neutral position. The corresponding positioning error keeps increasing passed the faulty joints as shown in e). c) Shows the same faulty i ² Snake with the fault tolerance feature activated.

It can be seen that the algorithm is able to find a solution to fit the faulty i ² Snake as close as possible to the desired path which would not be possible with a simple approach such as stopping the actuation of the faulty tendon as shown in Figure 11 b). f) Shows the positioning error that increases around the faulty joints. An important matter to consider while performing fault detection is that, although a faulty joint can be detected, knowing the faulty joint angle is important to accurately perform compensation. In Figure 11, it was assumed that all the joints would stay almost straight as the hardware running inside of the i ² Snake (instrument channels, Bowden cables, camera’s wires, sheath, etc.) would act as a spring keeping the joints almost straight as in traditional endoscopy.

An alternative would be to estimate the faulty joint angle by interpolating the angles between the adjacent healthy joints. This assumption was made only to show the potential of fault tolerance. Real-life applications would either require individual internal joint sensing, e.g., optical sensing (Schmitz et al., 2017), FBG sensors (Liu et al., 2015) or external shape sensing with a sheath that can also sense external contacts (Wasylczyk et al., 2018). In any of these cases, MOVE would be able to perform the compensation as long as the faulty joint angles can be estimated.

The performance of the solver was evaluated to assess its real-time capabilities. To evaluate the real-time performance, a trajectory was pre-recorded using the simulator. This trajectory recorded in the master ‘space’ is then used to generate a trajectory in the “slave” space that can be fed to the solver (solver trajectories are robot dependent as they depend on the amount of joints). The “slave” trajectory is then fed to the solver at varying frequencies (from 1Hz to 2 kHz) which means that the higher the frequency, the less time the solver has to converge to a solution. In the case of the extended i ² Snake, the full-body Jacobian size is (165 × 54), and reaches (381 × 126) with n = 126 in the most complex robot architecture tested.

Figure 12 illustrates the corresponding results. The evaluation was performed using several i ² Snake-like robots architecture with increasing complexity with ‘n’ being the amount of joint variables before mechanical coupling. The plots show the averaged RMS error between the link position and the path. The initial starting error is not zero as the i ² Snake is not able to exactly follow the path. For the extended i ² Snake, the error does not significantly change up to 700 Hz and can run at more than 1 kHz with sub-mm accuracy. The error of more complex robots is lower at low frequency as they have more joints and hence the average error per joint is decreased.

There are various parameters that can be tuned in the MOVE framework. The error tolerance function and parameters can be tuned to either match the size of the lumen or to define a soft limit, hence allowing to use the available space. This function was implemented to ensure a smooth transition between the forbidden and the allowed regions (rather than a ‘bang-bang’ type of control), but other types of functions could also be used. While in the allowed region, the other parameters such as tendon-stress reduction will induce joint motion, but once in the forbidden region, the solver will always aim to converge to the minimal error and hence override these factors.

Regarding the path sampling resolution, it is defined in the workspace of the robot. It determines the level of precision of the path taken by the robot. Hence, it is task dependent rather than robot dependent. If the resolution is 1 mm, additional control points will only be added if the robot motion is greater than 1 mm. For navigation through the oesophagus, a sub-mm resolution is sufficient clinically. In the case of lung, endovascular, or brain application, this resolution should be smaller.

The fact that the i ² Snake cannot always satisfy the desired path is an intrinsic design limitation which cannot be compensated by a simplistic FTL navigation. The i ² Snake cannot intrinsically perform FTL motion since its joints can only move in one direction. The concept of MOVE is to allow the use of the surrounding space in a controlled way. In Figure 6A, we show the problem of pure FTL navigation with no controlled error. The resulting behaviour of the robot is unpredictable as it is dictated by the kinematics equations of the solver and different joint configurations will result in different behaviours. With the additional features of MOVE introduced such as error tolerance and weights, it is now possible to control the error between the robot and the path. This behaviour is much safer for surgical applications.

The use of sensors could further improve the safety by avoiding excessive pressure on the lumen. In practice, the surgeon must be aware of the limitations of the system (as in traditional surgery or robotic surgery; all instruments have limitations) but it should be handled by the robot and not the operator. The use of sensors could alert if an excessive pressure is detected. If the lumen is closing on each side, which is likely to happen during endoscopic application, the ideal approach would be to balance the pressure on each side to ensure a safe navigation. In all cases the system should have a maximum pressure allowed to avoid the tearing of the lumen.

During endoscopy, excessive forces against sharp bends such as in the colon can result in patient injury. In the context of MOVE, this risk is significantly reduced, as the robotic endoscope will actively follow the path without relying on anatomical structures as a guide. However, it is inevitable that the patient moves during the procedure, which could result in unexpected anatomical change and excessive contact force between the robot and the anatomy. This can be avoided if the robot is equipped with force or contact sensors. The sensor information can be used to alter the virtual robot fitting, so the new fitting point is away from the head’s path and in a contact free area.

This process could consist of translating the head path segment where a contact was detected, and to translate this segment opposite to the contact and normal to the contact direction as illustrated in Figure 15. The new fitting point would be located at the intersection between the translated path segment and the link’s sphere as in the normal fitting procedure. Once a safe point is found, the fitting could continue as normal.