Bond Graph Modelling and Control of Hyper-Redundant Miniature Robot for In-Vivo Biopsy

Abstract

The introduction of robot-assisted minimally invasive techniques to general surgery for performing biopsy and abdominal cavity exploration is a significant development in medical science. The inclusion of in-vivo robot for abdominal biopsy is an achievement in this area and required a constant persuasion for the improvisation of the design and various controlling aspects in it as there is a need of a miniature in-vivo robot as an exploration tool which can be easily controlled, and has high mobility. Development of such devices needs careful modelling and simulation before manufacturing the product for better results. This chapter describes the modelling and control aspects of the miniature hyper-redundant robot for stomach biopsy.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

1 Introduction

Endoscopy involves a procedure that uses a specialized tool to view, take biopsy sample, and operate on the abdominal cavity of human body when required. The procedure allows a surgeon to examine the abdominal cavity to ascertain the cause of discomfort reported by the patient, and for remedial action. It is performed with an endoscope, which is a flexible tube with all attached accessories required for biopsy and viewing. Gastroscopy is a type of endoscopy. It is a symptomatic endoscopic procedure that examines the gastrointestinal tract. It is considered a minimally invasive procedure as it does not require any incision into the body and does not require any significant recovery time after the procedure. Conventional endoscopy complicates the task of the surgeon due to factors like lack of dexterity, poor viewing angles, problems in manoeuvring the endoscope inside the abdominal cavity for exploration, and controlling the tip of the flexible endoscope tube to reach at the exact point of interest.

The adoption of in-vivo robots in conjunction with laparoscopic procedure involves the insertion of a robot through strategically located incision to place it properly at the operating location in the abdominal cavity [1]. Laparoscopic in-vivo procedure significantly reduces patient trauma from a long hospital stay and expensive medical bills as compared to the conventional open surgery . Additionally, it also eliminates the complications associated with surgeon’s tremor during the traditional procedure. However, these robots pose problems of insertion, retraction, and controlling of the robot.

In order to address the above problems there is a growing awareness to combine the robot with the endoscope in a procedure that may be called robotized endoscopy. The main advantage of robotized endoscopy is that it is incision-free and hence bereft of the difficulties associated with insertion and withdrawal of robot encountered in laparoscopic procedures. However the challenge lies in developing a miniature robot that is small enough to be carried by one of the endoscope channels and yet has enough manoeuvrability for executing the desired surgical procedures. Research in robotized endoscopy is still in its nascent stage and researchers round the globe are working on issues of concern such as compromised dexterity, limited visibility for the surgeon when performing the task, fewer degrees of freedom (DOF), and limited manoeuvrability.

The size of the robot should be such that available tool channel in the endoscope can be used to place the robotized manipulator in it. The endoscope is stationed at the entrance to the abdominal cavity, the robot comes out and performs its desired task of taking biopsy at a particular location by means of an effective inverse kinematic solution and then comes back to its home position in the endoscope.

Several robotic systems have been developed over the years to increase the surgeon's dexterity by masterly manipulating laparoscopic tools [2] where surgical dexterity refers to the ability to steer devices such as needles, rigid laparoscopic tools, and robotic tools [3]. To counter the problem of limitation on dexterity and to obtain high flexibility and manoeuvrability hyper-redundant robots are preferred over conventional ones. These types of robots are termed so, because their number of actuatable DOF is much higher than the DOF of their intended workspace [4]. Robots akin to snake, tentacles, or elephant trunks are examples of hyper-redundant robots [3, 5].

The efficiency of robotized surgical manipulator also depends on how it moves inside the abdominal environment in a controlled manner. Conventional endoscopes rely on the expertise of the surgeon to move the forceps tip to the desired location by rotating the knob in the endoscope. It is a tedious process and the surgeon faces considerable difficulty in manoeuvring the endoscope during forceps tip positioning at the location of interest. For the motion of tip of the surgical robot from one desired position to another, a proper inverse kinematic analysis is essential. Therefore there is a progressive need to develop a simple but effective inverse kinematic solution to place the surgical robot tip at the desired point of interest with minimum error.

Controlling of the hyper-redundant manipulator is a challenging task and is one of the major concerns in robotized endoscopy. During biopsy it is required to move the manipulator in the abdominal cavity environment and to place its tip at the desired point of interest, without damaging the healthy tissues. For this a proper control scheme for trajectory and force control are required and various strategies including hybrid position and force control strategies have been addressed by researchers in this regard [6]. In robotized endoscopy, the forceps tool can be moved to follow a certain path, by employing a trajectory controller which should be competent enough to ensure that the tip of the robotic manipulator follows the desired trajectory inside abdominal cavity.

2 Forward Kinematic Analysis of the In-Vivo Robot

Redundant robots have high dexterity and manoeuvrability as compared to conventional one. However, motion planning and controlling of redundant manipulators have always been a challenging task for researchers. To carry out analysis of a robot, the first important step is the creation of its dynamic model. Dynamic models are important to have feed-forward information for controlling the manipulators actions, by controlling the related actuators through interactive controlling software. This helps in

• Quick and correct motion of the manipulator along a predefined or designed path.

• Effective compensation system for inertia, centrifugal, gravity, and Coriolis components during motion.

Dynamic modelling of robot is essential for its kinematic analysis to obtain the transformer moduli required to develop the bond graph model. Mostly three methodologies have been adopted for this purpose namely,

• Lagrange–Euler (L-E) dynamic formulation

• Iterative Newton–Euler (N-E) dynamic formulation

• Bond Graph approach.

The bond graph modelling is simpler, and the effort equations derived for the robot are in single scalar closed form for each joint and can be used for online computations. These equations are always faster and more efficient than the iterative and recursive form of equations. Above all the modelling is easier, modular in nature, and gives physical insight and intuitive feeling of the dynamics. Also bond graph modelling is a unified approach, well suited for modelling of multi-domain discipline (due to the presence of mechanical, electrical, thermal, mechatronics, etc.) systems. It allows precise and independent nature of all the domains, with exchange of power in the system in the form of storage, dissipation, and transformation. The modelling of system is flexible as it allows graphical addition of friction, stiffness-effect elements as and when required [7].

The first step in the kinematic analysis is to assign frames to the in-vivo robot model. The frame assignment to the in-vivo robot is shown in Fig. 13.1. A coordinate frame is attached to each link . The bush is considered as link 0 and inertial frame {0} is attached to the bush. The subsequent frames are attached to the base of the coupler, the links, and to the clipper joint, respectively. The joint between link i and $i+1$ terms as joint $i+1$, e.g., the frame i is attached to link i. Standard D-H conventions have been used [8], for describing the frames. The notations are given in Table 13.1. The Denavit–Hartenberg notations basically depict the link and joint parameters. The overall homogeneous transformation matrix based on its general form [9] can be expressed as:

$$ {}_{11}{}^0T=\left[\begin{array}{cccc}\hfill c\phi {c}_{1234}\hfill & \hfill -c\phi {s}_{1234}\hfill & \hfill s\phi \hfill & \hfill {L}_1c\phi {c}_1+{L}_2c\phi {c}_{12}+{L}_3c\phi {c}_{123}+{L}_4c\phi {c}_{1234}\hfill \\ {}\hfill s\phi {c}_{1234}\hfill & \hfill -s\phi {s}_{1234}\hfill & \hfill -c\phi \hfill & \hfill {L}_1s\phi {c}_1+{L}_2s\phi {c}_{12}+{L}_3s\phi {c}_{123}+{L}_4s\phi {c}_{1234}\hfill \\ {}\hfill {s}_{1234}\hfill & \hfill {c}_{1234}\hfill & \hfill 0\hfill & \hfill \left(L+d\right)+{L}_1{s}_1+{L}_2{s}_{12}+{L}_3{s}_{123}+{L}_4{s}_{1234}\hfill \\ {}\hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right] $$

(13.1)

From Eq. (13.1) the manipulator tip positions can be expressed as follows:

$$ {X}_{\mathrm{tip}}=c\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}+{L}_4{c}_{1234}\right) $$

(13.2)

$$ {Y}_{\mathrm{tip}}=s\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}+{L}_4{c}_{1234}\right) $$

(13.3)

$$ {Z}_{\mathrm{tip}}=\left(L+d\right)+\left({L}_1{s}_1+{L}_2{s}_{12}+{L}_3{s}_{123}+{L}_4{s}_{1234}\right) $$

(13.4)

The manipulator is a connected chain of links, capable of motion relative to its neighbour link . The velocity of the link $i+1$ will be equal to the velocity of link i and new velocity component added by joint $i+1$. Since translational velocity is associated with a point, and angular velocity is associated with a body, the term “velocity of a link” signifies the translational velocity of the origin frame attached to that link, in addition to the rotational velocity of that link. It is evident that the rotational velocities may be added when both angular velocity vectors are written with respect to the same frame.

Fig. 13.1

Frame assignments to the in-vivo manipulator

Table 13.1 D-H parameters of the in-vivo robotic manipulator

The angular velocity of the link $i+1$ is the addition of the angular velocity of link i and a component caused by rotational velocity of joint $i+1$, with respect to the inertial frame {0}. So, the angular velocity relation of link $i+1$ with respect to inertial frame {0} and expressed in frame $i+1$ coordinate, is given by,

$$ {}^{i+1}\left({}^0\omega_{i+1}\right)={}_i{}^{i+1}R{}^i\left({}^0\omega_i\right)+{\overset{.}{\theta}}_{i+1}{}^{i+1}\widehat{Z}_{i+1}\kern1em \left(\mathrm{f}\mathrm{o}\mathrm{r}\kern0.5em \mathrm{r}\mathrm{o}\mathrm{tational}\kern0.5em \mathrm{joint}\right) $$

(13.5)

$$ {}^{i+1}\left({}^0\omega_{i+1}\right)={}_i{}^{i+1}R{}^i\left({}^0\omega_i\right)\kern1em \left(\mathrm{f}\mathrm{o}\mathrm{r}\kern0.5em \mathrm{translational}\kern0.5em \mathrm{o}\mathrm{r}\kern0.5em \mathrm{prismatic}\kern0.5em \mathrm{joint}\right) $$

(13.6)

In the present analysis the bush of in-vivo robot is considered as the inertial frame {0} so, using Eqs. (13.5) and (13.6) angular velocity of the links can be derived by substituting for different values of i. Therefore substituting the values of i the angular velocities obtained are as follows:

$$ {}^1\left({}^0\omega_1\right)={}^2\left({}^0\omega_2\right)={}^3\left({}^0\omega_3\right)={}^4\left({}^0\omega_4\right)={}^5\left({}^0\omega_5\right)={\left[\begin{array}{ccc}\hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right]}^T $$

(13.7)

$$ {}^6\left({}^0\omega_6\right)={\left[\begin{array}{ccc}\hfill 0\hfill & \hfill 0\hfill & \hfill \overset{.}{\phi}\hfill \end{array}\right]}^T $$

(13.8)

$$ {}^7\left({}^0\omega_7\right)={\left[\begin{array}{ccc}\hfill {s}_1\overset{.}{\phi}\hfill & \hfill {c}_1\overset{.}{\phi}\hfill & \hfill {\overset{.}{\theta}}_1\hfill \end{array}\right]}^T $$

(13.9)

$$ {}^8\left({}^0\omega_8\right)={\left[\begin{array}{ccc}\hfill {s}_{12}\overset{.}{\phi}\hfill & \hfill {c}_{12}\overset{.}{\phi}\hfill & \hfill {\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\hfill \end{array}\right]}^T $$

(13.10)

$$ {}^9\left({}^0\omega_9\right)={\left[\begin{array}{ccc}\hfill {s}_{123}\overset{.}{\phi}\hfill & \hfill {c}_{123}\overset{.}{\phi}\hfill & \hfill {\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\hfill \end{array}\right]}^T $$

(13.11)

$$ {}^{10}\left({}^0\omega_{10}\right)={\left[\begin{array}{ccc}\hfill {s}_{1234}\overset{.}{\phi}\hfill & \hfill {c}_{1234}\overset{.}{\phi}\hfill & \hfill {\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3+{\overset{.}{\theta}}_4\hfill \end{array}\right]}^T $$

(13.12)

Similarly the linear velocity of the origin of the frame $\left\{i+1\right\}$ will be equal to the sum of the origin of frame {i} and a new component caused by rotational velocity of the link i and is expressed as:

$$ {}^{i+1}\left({}^0V_{i+1}\right)={}_i{}^{i+1}R\left[{}^i\left({}^0V_i\right)+{}^i\left({}^0\omega_i\right)\times {}^i\left({}^iP_{i+1}\right)\right]\kern1em \left(\mathrm{f}\mathrm{o}\mathrm{r}\kern0.5em \mathrm{r}\mathrm{o}\mathrm{tational}\kern0.5em \mathrm{joint}\right) $$

(13.13)

$${}^{i+1}\left({}^0V_{i+1}\right)={}_i{}^{i+1}R\left[{}^i\left({}^0V_i\right)+{}^i\left({}^0\omega_i\right)\times {}^i\left({}^iP_{i+1}\right)\right]+{\overset{.}{d}}_{i+1}\times {}^{i+1}\widehat{Z}_{i+1}\kern1em \left(\mathrm{f}\mathrm{o}\mathrm{r}\kern0.5em \mathrm{prismatic}\kern0.5em \mathrm{joint}\right) $$

(13.14)

The position ${}^i\left({}^iP_{i+1}\right)$ represents the position of the origin of the frame $\left\{i+1\right\}$ with respect to the frame {i} expressed in the ith frame, and the velocity ⁱ(⁰ V _i ) represents the velocity of the origin of the frame {i} with respect to the inertial frame {0} and expressed in frame i coordinate, ${}_i{}^{i+1}R$ represents the orientation of frame {i} with respect to the frame $\left\{i+1\right\}$. Translational velocities of each frame can be obtained by substituting different values of i in Eq. (13.13) or Eq. (13.14), depending upon whether the joint is rotary or prismatic, respectively. The translational velocities are obtained by substituting different values of i and they are expressed with respect to the inertial frame and are expressed as follows:

$$ {}^0\left({}^0V_1\right)={}^0\left({}^0V_2\right)={}^0\left({}^0V_3\right)={}^0\left({}^0V_4\right)={}^0\left({}^0V_5\right)={}^0\left({}^0V_6\right)={}^0\left({}^0V_7\right)={\left[\begin{array}{ccc}\hfill 0\hfill & \hfill 0\hfill & \hfill \overset{.}{d}\hfill \end{array}\right]}^T $$

(13.15)

$$ {}^0\left({}^0V_8\right)=\left[\begin{array}{c}\hfill -{L}_1c\phi {s}_1{\overset{.}{\theta}}_1-{L}_1s\phi {c}_1\overset{.}{\phi}\hfill \\ {}\hfill -{L}_1s\phi {s}_1{\overset{.}{\theta}}_1+{L}_1c\phi {c}_1\overset{.}{\phi}\hfill \\ {}\hfill \overset{.}{d}+{L}_1{c}_1{\overset{.}{\theta}}_1\hfill \end{array}\right] $$

(13.16)

$$ {}^0\left({}^0V_9\right)=\left[\begin{array}{c}\hfill -{L}_1c\phi {s}_1{\overset{.}{\theta}}_1-{L}_2c\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-s\phi \left({L}_1{c}_1+{L}_2{c}_{12}\right)\overset{.}{\phi}\hfill \\ {}\hfill -{L}_1s\phi {s}_1{\overset{.}{\theta}}_1-{L}_2s\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)+c\phi \left({L}_1{c}_1+{L}_2{c}_{12}\right)\overset{.}{\phi}\hfill \\ {}\hfill \overset{.}{d}+{L}_1{c}_1{\overset{.}{\theta}}_1+{L}_2{c}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)\hfill \end{array}\right] $$

(13.17)

$$ {}^0\left({}^0V_{10}\right)=\left[\begin{array}{c}\hfill \begin{array}{l}-{L}_1c\phi {s}_1{\overset{.}{\theta}}_1-{L}_2c\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_3c\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}-s\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}\right)\overset{.}{\phi}\end{array}\hfill \\ {}\hfill \begin{array}{l}-{L}_1s\phi {s}_1{\overset{.}{\theta}}_1-{L}_2s\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_3s\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}+c\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}\right)\overset{.}{\phi}\end{array}\hfill \\ {}\hfill \overset{.}{d}+{L}_1{c}_1{\overset{.}{\theta}}_1+{L}_2{c}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)+{L}_3{c}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\hfill \end{array}\right] $$

(13.18)

$$\begin{aligned} {}^0\left({}^0V_{11}\right)=\left[\begin{array}{c}\begin{array}{l}-{L}_1c\phi {s}_1{\overset{.}{\theta}}_1-{L}_2c\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_3c\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}-{L}_4c\phi {s}_{1234}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3+{\overset{.}{\theta}}_4\right)\\-s\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}+{L}_4{c}_{1234}\right)\overset{.}{\phi}\\ \end{array} \\ \begin{array}{l}-{L}_1s\phi {s}_1{\overset{.}{\theta}}_1-{L}_2s\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_3s\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ -{L}_4s\phi {s}_{1234}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3+{\overset{.}{\theta}}_4\right)\\ +c\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}+{L}_4{c}_{1234}\right)\overset{.}{\phi}\\ \end{array}\\ \overset{.}{d}+{L}_1{c}_1{\overset{.}{\theta}}_1+{L}_2{c}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)+{L}_3{c}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\+{L}_4{c}_{1234}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3+{\overset{.}{\theta}}_4\right)\end{array}\right] \end{aligned}$$

(13.19)

For preparing the dynamic bond graph model of the in-vivo robot the velocities of the centre of mass of each link are also required therefore these velocities can be expressed as:

$$ {}^0\left({}^0V_{G_7}\right)=\left[\begin{array}{c}\hfill -{L}_{G_1}c\phi {s}_1{\overset{.}{\theta}}_1-{L}_{G_1}s\phi {c}_1\overset{.}{\phi}\hfill \\ {}\hfill -{L}_{G_1}s\phi {s}_1{\overset{.}{\theta}}_1+{L}_{G_1}c\phi {c}_1\overset{.}{\phi}\hfill \\ {}\hfill \overset{.}{d}+{L}_{G_1}{c}_1{\overset{.}{\theta}}_1\hfill \end{array}\right] $$

(13.20)

$$ {}^0\left({}^0V_{G_8}\right)=\left[\begin{array}{c}\hfill -{L}_1c\phi {s}_1{\overset{.}{\theta}}_1-{L}_{G_2}c\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-s\phi \left({L}_1{c}_1+{L}_{G_2}{c}_{12}\right)\overset{.}{\phi}\hfill \\ {}\hfill -{L}_1s\phi {s}_1{\overset{.}{\theta}}_1-{L}_{G_2}s\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)+c\phi \left({L}_1{c}_1+{L}_{G_2}{c}_{12}\right)\overset{.}{\phi}\hfill \\ {}\hfill \overset{.}{d}+{L}_1{c}_1{\overset{.}{\theta}}_1+{L}_{G_2}{c}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)\hfill \end{array}\right] $$

(13.21)

$$ {}^0\left({}^0V_{G_9}\right)=\left[\begin{array}{c}\hfill \begin{array}{l}-{L}_1c\phi {s}_1{\overset{.}{\theta}}_1-{L}_2c\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_{G_3}c\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}-s\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_{G_3}{c}_{123}\right)\overset{.}{\phi}\\ {}\end{array}\hfill \\ {}\hfill \begin{array}{l}-{L}_1s\phi {s}_1{\overset{.}{\theta}}_1-{L}_2s\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_{G_3}s\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}+c\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_{G_3}{c}_{123}\right)\overset{.}{\phi}\\ {}\end{array}\hfill \\ {}\hfill \overset{.}{d}+{L}_1{c}_1{\overset{.}{\theta}}_1+{L}_2{c}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)+{L}_{G_3}{c}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\hfill \end{array}\right] $$

(13.22)

These equations are obtained by substituting ${L}_1={L}_{G_1}$ in Eq. (13.16), ${L}_2={L}_{G_2}$ in Eq. (13.17), ${L}_3={L}_{G_3}$ in Eq. (13.18), and ${L}_4={L}_{G_4}$ in Eq. (13.19), respectively.

The transformer moduli obtained from the kinematic analysis has been used for the bond graph modelling of the in-vivo robot.

$$ {}^0\left({}^0V_{G_{10}}\right)=\left[\begin{array}{c}\hfill \begin{array}{l}-{L}_1c\phi {s}_1{\overset{.}{\theta}}_1-{L}_2c\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_3c\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}-{L}_{G_4}c\phi {s}_{1234}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3+{\overset{.}{\theta}}_4\right)\\ {}-s\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}+{L}_{G_4}{c}_{1234}\right)\overset{.}{\phi}\\ {}\end{array}\hfill \\ {}\hfill \begin{array}{l}-{L}_1s\phi {s}_1{\overset{.}{\theta}}_1-{L}_2s\phi {s}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)-{L}_3s\phi {s}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}-{L}_{G_4}s\phi {s}_{1234}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3+{\overset{.}{\theta}}_4\right)\\ {}+c\phi \left({L}_1{c}_1+{L}_2{c}_{12}+{L}_3{c}_{123}+{L}_{G_4}{c}_{1234}\right)\overset{.}{\phi}\\ {}\end{array}\hfill \\ {}\hfill \begin{array}{l}\overset{.}{d}+{L}_1{c}_1{\overset{.}{\theta}}_1+{L}_2{c}_{12}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2\right)+{L}_3{c}_{123}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3\right)\\ {}+{L}_{G_4}{c}_{1234}\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\theta}}_3+{\overset{.}{\theta}}_4\right)\end{array}\hfill \end{array}\right] \vspace*{-1pc}$$

(13.23)

3 Bond Graph Modelling of the In-Vivo Robot

Modulated transformer moduli in terms of the translational and angular velocities required to create the bond graph model are obtained from the kinematic analysis of the robot from the preceding section. Based on these transformer moduli the dynamic model of the in-vivo robot was prepared in bond graph [7, 10, 11, 12] which represents the power exchange portrait of the system. Complete bond graph of the in-vivo robot is shown in Fig. 13.2. In the present analysis, bond graph modelling and its simulation are performed using bond graph modelling software SYMBOLS Shakti^®, a bond graph modelling software.

Fig. 13.2

Bond graph model of the in-vivo robot with individual joint actuation

The rotational dynamics of different links of the in-vivo robot are represented by Euler Junction Structure EJS, shown on the right side of the model. Translational dynamics of the link are shown on the left side of the model. The centre of gravity CG velocity of links depends on the link inertia. Therefore in the bond graph model I element is attached at velocity junctions representing the CG velocity of links. Tip velocities are indicated on the top of the model. Integrators are used to integrate the velocity components to obtain the tip positions, i.e., X _tip, Y _tip and Z _tip. The proficiency of the wire actuation is tested in the present modelling by actuating the joints independently and then by actuating the joints simultaneously. In case of the independent joint actuation individual efforts are provided to each joint as shown in Fig. 13.2. The efforts SE are assumed to be equivalent to the wire actuation effort. For rotation of joint a very negligible joint resistance of 0.001 Nm/ (rad/s) is given and when it is required to lock the joint the joint resistance is set at 10,000 Nm/ (rad/s).

3.1 Independent Joint Actuation Through Wire Actuation

The initial validity of the bond graph model was tested without wire actuation with a four-scaled model. For the simulation various parameters used are as enlisted in Table 13.2. The initial configuration of the robot is shown in Fig. 13.3. Initially the validity of the bond graph was checked without wire actuation. A torque of 2 Nm was applied on 1st joint and it was rotated while all other joints were locked at zero degree initial value. As it can be seen a circle of 0.176 m radius (sum of four planar link lengths) was obtained in X-Y plane, as shown in Fig. 13.4. A torque of 2 Nm was applied on 1st joint and it was rotated while all other joints were locked at zero degree initial value.

Table 13.2 Simulation parameters for in-vivo robot without wire actuation

Fig. 13.3

Initial configuration of the in-vivo robot

Fig. 13.4

Tip trajectory of in-vivo robot by rotating first joint with all other joints locked at zero degree initial value

Tip trajectory obtained is a circle of 0.176 m radius in X-Z plane as plotted in Fig. 13.5. Physically this configuration is possible when the manipulator is out of the tool channel and is inside the stomach model.

Fig. 13.5

Tip trajectory by rotating second joint with all other joints locked at zero degree initial value

3.2 Simultaneous Joint Actuation Through Wire Actuation

For simultaneous joint wire actuation efforts are provided to each joint through the wire actuation as shown in Fig. 13.6. Wire actuation system is integrated in the bond graph model and shown on the top right-hand side of the figure. Wire flexibility in the wire actuation system is represented by K _wire and R _wire. Modulated transformer moduli obtained from the kinematic analysis have been used to construct the bond graph model and are shown in Fig. 13.6. Translational dynamics is indicated on the left-hand side of the figure and rotational dynamics is indicated on the right-hand side of the figure. The modulated transformer moduli for the bond graph are shown in Table 13.3. For the simulation various parameters used are as enlisted in Table 13.4. The initial configuration was same as shown in Fig. 13.3.

Fig. 13.6

Bond graph model of in-vivo robot with simultaneous joint actuation

Table 13.3 Simulation parameters for simultaneous joint actuation

Table 13.4 Transformer moduli for the bond graph model of the virtual link based controller

The wire actuation motion for obtaining the 3rd DOF was simulated by activating the upward and downward wire actuation and the tip trajectory obtained from the simulation is shown in Figs. 13.7 and 13.8, where Fig. 13.7 indicates the upward wire actuation and Fig. 13.8 shows the downward wire actuation. During the actuation process all the joints were given an equal joint rotation.

Fig. 13.7

Tip trajectory by actuating wire with tip moving in upward direction

Fig. 13.8

Tip trajectory by actuating wire with tip moving in downward direction

4 Experimental Validation of Wire Actuation Simulation in Four-Scaled Model

Experimentation was carried out to validate the simulation results obtained for the tip positions during the upward and downward wire actuation motion. Different tip positions recorded during the upward wire actuation and four of them are shown in Fig. 13.9.

Fig. 13.9

Upward direction of motion of wire actuated four-scaled model (a) initial tip position, (b) 1st intermediate tip position, (c) 2nd intermediate tip position, and (d) final tip position

Initial tip position was recorded at (0.176, 0.044) m shown in Fig. 13.9a, 1st intermediate tip position was recorded at (0.162, 0.079) m shown in Fig. 13.9b, 2nd intermediate tip position was located at (0.108, 0.106) m shown in Fig. 13.9c, final tip positions were recorded at (0.067, 0.084) m shown in Fig. 13.9d. It can be seen by comparing Figs. 13.7 and 13.9 that in experimentation the tip followed the same path in a very close approximation. Similarly the tip positions during the downward wire actuation have been shown in Fig. 13.10. Initial tip position was recorded at (0.176, 0.044) m as shown in Fig. 13.10a, 1st intermediate tip position is at (0.148, −0.033) m shown in Fig. 13.10b, 2nd intermediate tip position is obtained at (0.074, −0.066) m shown in Fig. 13.10c, final tip position is located and situated (0.027, −0.039) m shown in Fig. 13.10d.

Fig. 13.10

Downward direction of motion of wire actuated four-scaled model (a) initial tip position, (b) 1st intermediate tip position, (c) 2nd intermediate tip position, and (d) final tip position

5 Experimental Validation of Wire Actuation Simulation in Actual-Scaled Model

Same experimentation was also carried out on actual scale in-vivo robot to validate the simulation results obtained for the tip positions during the upward and downward wire actuation motion. Different tip positions were recorded during the upward wire actuation and four of them are shown in Fig. 13.11. The initial tip position was recorded at (0.044, 0.011) m shown in Fig. 13.11a, 1st intermediate tip position was recorded at (0.0405, 0.0197) m shown in Fig 13.11b, 2nd intermediate tip position was located at (0.0225, 0.0255) m shown in Fig. 13.11c, final tip positions were recorded at (0.0167, 0.021) m shown in Fig. 13.11d. It can be seen by comparing Figs. 13.9 and 13.11 that in experimentation the tip followed the same tip position in a scaled manner.

Fig. 13.11

Upward direction of motion of wire actuated actual scale model (a) initial tip position, (b) 1st intermediate tip position, (c) 2nd intermediate tip position, and (d) final tip position

Similarly the tip positions during the downward wire actuation have been shown in Fig. 13.12. Initial tip position was recorded at (0.044, 0.011) m as shown in Fig. 13.12a, 1st intermediate tip position is at (0.037, −0.008) m shown in Fig. 13.12b, 2nd intermediate tip position is obtained at (0.0277, −0.015) m shown in Fig. 13.12c, final tip position is located and situated at (0.0675, −0.0097) m shown in Fig. 13.12d.

Fig. 13.12

Downward direction of motion of wire actuated actual scale model (a) initial tip position, (b) 1st intermediate tip position, (c) 2nd intermediate tip position, and (d) final tip position

6 Inverse Kinematic Analysis

In this section the inverse kinematic analysis of the proposed 4-DOF in-vivo robot has been explained. The tip Cartesian coordinates can be obtained from the kinematic analysis as expressed in Eqs. (13.2), (13.3), and (13.4). Now, assuming all the links of the manipulator to be equal, the link lengths can be expressed as,

${L}_1={L}_2={L}_3={L}_4={L}_l$ and a planar case, i.e., $\phi =0$ and with all joint angles being equal, i.e.,

$$ {\theta}_1={\theta}_2={\theta}_3={\theta}_4=\theta, $$

(13.24)

Equations (13.2), (13.3), and (13.4) can be expressed as:

$$ {X}_{\mathrm{tip}}={L}_l\left( \cos \theta + \cos 2\theta + \cos 3\theta + \cos 4\theta \right) $$

(13.25)

$$ {Y}_{\mathrm{tip}}=0 $$

(13.26)

$$ {Z}_{\mathrm{tip}}=\left(L+d\right)+{L}_l\left( \sin \theta + \sin 2\theta + \sin 3\theta + \sin 4\theta \right) $$

(13.27)

Assuming there is no translational motion, i.e., $d=0$ and ${L}_l=0.044\kern0.5em \mathrm{m}$ (for four-scaled model), the joint angle θ can be expressed for a given value of X _tip and Z _tip as:

$$ \theta {=} \arccos \left\{\frac{1}{3}\left(\frac{\sqrt[3]{3\sqrt{3}\times \sqrt{27{A}^2{-}4A}{+}27A{-}2}}{\sqrt[3]{2}}{+}\frac{\sqrt[3]{2}}{\sqrt[3]{3\sqrt{3}\times \sqrt{27{A}^2{-}4A}{+}27A{-}2}}{-}1\right)\right\} $$

(13.28)

where

$$ A=64.566\left({X}_{\mathrm{tip}}^2+{Z}_{\mathrm{tip}}^2\right)-5.682{Z}_{\mathrm{tip}}+0.125 $$

(13.29)

6.1 Determination of Joint Angle in Terms of Wire Length

A relational approach has been established to express joint angle in terms of the length of outer and inner wire used in actuation. In the analysis it is assumed that the wire actuated portion comprising of four articulated links as shown in Fig. 13.13 is a perfect arc of a circle.

Fig. 13.13

Representation of wire actuated section of in-vivo robot arm

Now considering a two link segment of the above entire section, i.e., link 1 and link 2 of joint angle θ ₂ shown in Fig. 13.14, and considering the wire actuation on the two link segment length of outer and inner wire length are as follows, respectively:

Fig. 13.14

Link segment between link 1 and link 2

$$\begin{aligned}{l}_2&={l}_a+{l}_b\ \mathrm{and},\\ {l}_1&={l}_c+{l}_d\end{aligned} $$

The relation between the outer and inner wire length can be obtained by considering the whole wire actuated portion as shown in Fig. 13.14., and with a known relation ${l}_a={l}_c$.

The outer wire length is

$$ {L}_o=4{l}_a+3{l}_b $$

(13.30)

and the inner wire length is

$$ {L}_o=4{l}_a+3{l}_b $$

(13.31)

From Fig. 13.14 the following geometrical relation can be deduced:

$$ \sin \frac{\theta_2}{2}=\frac{l_b-{l}_d}{2D} $$

(13.32)

where D is the diameter of link . Substituting the values of l _b and l _d from Eqs. (13.30) and (13.31) in Eq. (13.32), we have

$$ \sin \frac{\theta_2}{2}=\frac{\left(\frac{L_o-4{l}_a}{3}\right)-\left(\frac{L_i-4{l}_c}{3}\right)}{2D} $$

(13.33)

Further simplifying the difference in outer and inner wire length can be expressed as:

$$ {L}_0-{L}_i=6D \sin \frac{\theta }{2} $$

(13.34)

If wire is wrapped on the same pulley, with direction of wrapping for outer wire to be opposite to that of inner wire, the length of wire wrapped on pulley is given by,

$$ {L}_0-{L}_i={r}_p.{\theta}_p $$

(13.35)

where r _p is the radius of pulley and θ _p is the pulley rotation. Substituting the value of ${L}_o-{L}_i$ from Eq. (13.34) in Eq. (13.35) the pulley rotation can be expressed as:

$$ {\theta}_p=\left(6D/{r}_p\right) sin\left(\theta /2\right) $$

(13.36)

Thus for a given X _tip and Z _tip one can find θ from Eq. (13.28) and for this θ, the value of pulley rotation θ _p in terms of tip positions can be obtained. A Proportional Derivative (PD) controller can be used to take the pulley to desired θ _p rotation, thus the robot can be moved to a desired position. To implement the inverse kinematics solution the motor voltage for a desired pulley rotation corresponding to desired tip location can be evaluated as:

$$ V={K}_p\left({\theta}_p-{\theta}_a\right)+{K}_v\left({\overset{.}{\theta}}_p-{\overset{.}{\theta}}_a\right) $$

(13.37)

where V is the voltage applied to the motor, K _p is proportional gain, K _V is derivative gain, θ _P is desired pulley rotation in radian, θ _a is the actual pulley rotation in radian, ${\overset{.}{\theta}}_P$ is desired angular velocity of the pulley, and ${\overset{.}{\theta}}_a$ is actual angular velocity of the pulley.

6.2 Implementation of the Control Scheme

Desired tip positions are given as input to the bond graph and the joint angles are evaluated from the relations. Then from the joint angle, difference in outer and inner wire length is evaluated and finally the pulley rotation required to cover the wire length difference is obtained. Then the voltage supplied to motor to rotate the pulley, to wrap and unwrap the wire on the opposite sides, for the tip of the robot to reach from its current position to the point of interest is calculated. This calculation is based on the PD control law. Voltage is given as input to the system. Evaluation of motor voltage for desired tip position of robot has been explained in the form of a flowchart shown in Fig. 13.15.

Fig. 13.15

Flow chart for tip motion control

6.3 Simulation and Experimental Results

Simulation results obtained for three different tip trajectories in workspace are shown in Fig. 13.16. Simulations have been carried out to move the tip from an initial tip position of ${X}_{\mathrm{tip}}=0.1565\kern0.5em \mathrm{m}$ and ${Z}_{\mathrm{tip}}=0.1170\kern0.5em \mathrm{m}$ to ${X}_{\mathrm{tip}}=-0.022\kern0.5em \mathrm{m}$ and ${Z}_{\mathrm{tip}}=-0.012\kern0.5em \mathrm{m}$, as shown by trajectory 1 in Fig. 13.16. Then the tip was moved from an initial position of ${X}_{\mathrm{tip}}=0.1047\kern0.5em \mathrm{m}$ and ${Z}_{\mathrm{tip}}=0.1688\kern0.5em \mathrm{m}$ to ${X}_{\mathrm{tip}}=-0.066\kern0.5em \mathrm{m}$ and ${Z}_{\mathrm{tip}}=0.013\kern0.5em \mathrm{m}$, as shown by trajectory 2 in Fig. 13.16. Further a tip movement from an initial tip position of ${X}_{\mathrm{tip}}=0.0381\kern0.5em \mathrm{m}$ and ${Z}_{\mathrm{tip}}=0.1862\kern0.5em \mathrm{m}$ to a final tip position of ${X}_{\mathrm{tip}}=0.022\kern0.5em \mathrm{m}$ and ${Z}_{\mathrm{tip}}=0.108\kern0.5em \mathrm{m}$ has been performed by trajectory 3 as shown in Fig. 13.16.

Fig. 13.16

Three different tip trajectories in workspace

First trajectory was obtained after simulating the system for 6.5 s; second trajectory was obtained after a simulation of 8.25 s, and the third trajectory after a simulation of 3.4 s. The tip velocities with respect to time for the above mentioned trajectories are shown in Fig. 13.17. In the plot an initial drift was recorded which was minimized later with time.

Fig. 13.17

X _tip and Z _tip trajectory versus time for (a) 1st trajectory, (b) 2nd trajectory, and (c) 3rd trajectory

Animations of the 4-DOF in-vivo robotic manipulator for the 1st, 2nd, and 3rd trajectory are shown in Fig. 13.18a–c, respectively. Different joint postures during the simulation from the initial position to the desired position are shown in these figures. Fig. 13.18a shows the animation for the first trajectory, Fig. 13.18b shows the animation for the second trajectory, and Fig. 13.18c shows the animation for the third trajectory.

Fig. 13.18

Animation results of 4-DOF in-vivo robot for (a) 1st trajectory, (b) 2nd trajectory, and (c) 3rd trajectory

6.4 Experimental Validation of Desired Tip Positions

Experiments have been carried out on the actual scale model of in-vivo robot to validate the simulation results obtained for desired tip positions. The desired tip positions were obtained in the experimentation by rotating the pulley through wire actuation . Experimentations were performed to validate the three different trajectories obtained in simulation. For the actual scale model the initial position was recorded at (0.039, 0.0292) m shown in Fig. 13.19a and the final tip position was obtained at (−0.005, −0.003) m shown in Fig. 13.19d. Intermediate tip locations are at (0.0375, 0.01575) m and at (0.0325, 0.0077) m are shown in Fig. 13.19b, c, respectively. Figure 13.20 indicates the initial, two intermediate, and final tip positions during the second trajectory for the actual-scaled model. The initial tip position was at (0.02675, 0.042) m shown in Fig. 13.20a and the final tip position was measured to be (−0.0165, 0.003) m shown in Fig. 13.20d. The intermediate tip positions recorded during the experiments are shown in Fig. 13.20b, c respectively. Similarly the tip positions for the 3rd trajectory of actual scale model were recorded and are shown in Fig. 13.21.

Fig. 13.19

Experimental results of tip position for 1st trajectory of actual scale model (a) initial, (b) 1st intermediate, (c) 2nd intermediate, and (d) final tip position

Fig. 13.20

Experimental results of tip position for 2nd trajectory of actual scale model (a) initial, (b) 1st intermediate, (c) 2nd intermediate, and (d) final tip position

Fig. 13.21

Experimental results of tip position for 3rd trajectory of actual scale model (a) initial, (b) 1st intermediate, (c) 2nd intermediate, and (d) final tip position

The initial tip position was recorded at (0.09, 0.0465) m shown in Fig. 13.21a, 1st intermediate tip position was recorded at (0.105, 0.04) m shown in Fig. 13.21b, 2nd intermediate tip position was obtained at (0.0085, 0.03) m shown in Fig. 13.21c, and the final tip position was located at (0.005, 0.02675) m plotted in Fig. 13.21d.

Comparisons between the trajectories obtained in simulation and from experimental results for the three cases are shown in Figs. 13.22, 13.23, and 13.24. Comparison between the simulation and experimental result obtained for the first trajectory is shown in Fig. 13.22. Similarly the comparison between the simulation and experimental results for the second and third trajectories is shown in Figs. 13.23 and 13.24, respectively. Difference between X and Z coordinates gives the deviation between simulated and experimental results for the three cases. This can be found from numerical data of simulated and experimental values

Fig. 13.22

Comparison between simulation and experimental result for 1st trajectory for four-scaled model

Fig. 13.23

Comparison between simulation and experimental result for 2nd trajectory for four-scaled model

Fig. 13.24

Comparison between simulation and experimental result for 3rd trajectory for four-scaled model

The comparison was done between the simulation and experimental results for the four-scaled model and the actual scale for the three trajectories. From the comparison of only experimental results between the four-scaled and actual scale model it was evident that the actual scale model follows the same proportionate trajectory as that of the four-scaled model.

The deviation between simulated and experimental results of X and Z coordinates for tip position in three different cases for the four-scaled model are $\left(2.4\times {10}^{-6},4.1\times {10}^{-6}\right)\kern0.5em m$ for the 1st trajectory, $\left(5.632\times {10}^{-4},4.25\times {10}^{-5}\right)$ m for the 2nd trajectory, and $\left(2.97\times {10}^{-5},2\times {10}^{-5}\right)$ m for the 3rd trajectory. In fact this is the actual condition faced by surgeons where they would be interested in taking the robot tip from one position to the desired position.

7 Trajectory Control Analysis of In-Vivo Robot

Controlling hyper-redundant robotic manipulators as in-vivo manipulators is a major concern for researchers working in this particular field. During biopsy it is required to move the manipulator in the abdominal cavity environment and to place its tip at the desired point of interest. For this a proper trajectory control scheme is required.

A trajectory control scheme for a planar hyper-redundant manipulator has been proposed. The scheme is based on a virtual link based controller. An example of three link planar manipulator is considered to illustrate the methodology. The 3-link planar manipulator was modelled using bond graph, and a controller, using the virtual link concept has been formulated. Relations between the real and virtual links were established to design the controller and the virtual link based controller was used to send effort signals to all the joints of 3-link planar manipulator upon receiving a corrective signal from PI controller. Different desired trajectory shapes were used to check the efficiency of the controller. Simulations and animations have been performed to test the competence of the controller.

7.1 Virtual Link Based Controller for Trajectory Control of 3-Link Planar Redundant Manipulator

The trajectory control of in-vivo robot is based on the virtual link based controller [13]. In the present approach the numbers of real links present in the controller are reduced to the minimum number of fictitious links, called virtual links. This proposed control scheme is applicable to any number of planar hyper-redundant manipulator links. A generalized solution technique for any number of planar links (more than one) has been discussed before its implementation to the 3-link planar redundant manipulator. For simplicity a 5-link planar manipulator example shown in Fig. 13.25 has been considered. The derivation of the controller can be explained in the following steps:

Fig. 13.25

Derivation of controller for a 5-DOF planar manipulator (a) Step I, (b) Step II, (c) Step III, and (d) Step IV

7.1.1 Step I

In the first step shown in Fig. 13.25a two real links L ₅ and L ₄ are replaced with a fictitious link of varying length ${L}_{v_1}$. The joint angle of the virtual link ${L}_{v_1}$ can be expressed as $\left({\zeta}_2+{\theta}_4\right)$, where θ ₄ is the angle of real link L ₄ with respect to its previous link L ₃, ζ ₂ is the angle between L ₄ and ${L}_{v_1}$. Also in this figure ζ ₁ is the angle between L ₅ and ${L}_{v_1}$. From the geometrical relations, the angles ζ ₁ and ζ ₂ can be deduced as,

$$ {\zeta}_1+{\zeta}_2={\theta}_5 $$

(13.38)

The virtual link ${L}_{v_1}$ can be expressed as,

$$ {L}_{v_1}^2={L}_4^2+{L}_5^2-2{L}_4{L}_5 \cos \left(\pi -{\theta}_5\right) $$

Simplifying the above equation we have,

$$ {L}_{v_1}=\sqrt{L_4^2+{L}_5^2+2{L}_4{L}_5 \cos {\theta}_5} $$

(13.39)

7.1.2 Step II

In the second step shown in Fig. 13.25b, the fictitious link ${L}_{v_1}$ and real link L ₃ are replaced with an imaginary link ${L}_{v_2}$. In this figure ζ ₃ is the angle between ${L}_{v_1}$ and ${L}_{v_2}$, and ζ ₄ is the angle between L ₃ and ${L}_{v_2}$. The geometrical relation between the angles can be expressed as,

$$ {\zeta}_3+{\zeta}_4={\zeta}_2+{\theta}_4 $$

(13.40)

Length of ${L}_{v_2}$ can be expressed as,

$$ {L}_{v_2}^2={L}_{v_1}^2+{L}_3^2-2{L}_{v_1}{L}_3 \cos \left\{\pi -\left({\zeta}_2+{\theta}_4\right)\right\} $$

Thus

$$ {L}_{v_2}=\sqrt{L_{v_1}^2+{L}_3^2+2{L}_{v_1}{L}_3 \cos \left({\zeta}_2+{\theta}_4\right)} $$

(13.41)

7.1.3 Step III

In the third step shown in Fig. 13.25c, the fictitious link ${L}_{v_2}$ and real link L ₂ are replaced with an imaginary link ${L}_{v_3}$. In this figure ζ ₅ is the angle between ${L}_{v_3}$ and ${L}_{v_2}$ whereas ζ ₆ is the angle between L ₂ and ${L}_{v_3}$. The geometrical relation between the angles can be expressed as,

$$ {\zeta}_5+{\zeta}_6={\zeta}_4+{\theta}_3 $$

(13.42)

The fictitious link length ${L}_{v_3}$ can be expressed as,

$$ {L}_{v_3}^2={L}_{v_2}^2+{L}_2^2-2{L}_{v_2}{L}_2 \cos \left\{\pi -\left({\zeta}_4+{\theta}_3\right)\right\}, $$

or

$$ {L}_{v_3}=\sqrt{L_{v_2}^2+{L}_2^2+2{L}_{v_2}{L}_2 \cos \left({\zeta}_4+{\theta}_3\right)} $$

(13.43)

7.1.4 Step IV

The real links of the 5-link manipulator are converted to one virtual and one real link, as shown in Fig. 13.25d. The length of the virtual link is expressed in Eq. (13.43), and the joint angle of this link is $\left({\zeta}_6+{\theta}_2\right)$. Using the above developed methodology a virtual link based controller to control the 3-link planar manipulator in a two-dimensional workspace has been designed. This controller will provide the necessary joint torque required to each joint for a given end-effector trajectory. The steps described above in Sects. 13.7.1.1, 13.7.1.2, 13.7.1.3, and 13.7.1.4 have been followed for the development of virtual link based controller for a 3-link planar manipulator shown in Fig. 13.26. The controller is composed of two links (one real and one imaginary) and two revolute joints. The joint angles (θ ₁, θ ₂, ζ ₁) define the virtual link manipulator configuration. In the virtual link based controller an imaginary link of length L _v has been used instead of two actual links of length L ₂ and L ₃. The length of the virtual link from the geometrical relation can be deduced as,

Fig. 13.26

Schematic diagram of virtual link based controller

$$ {L}_v^2={L}_2^2+{L}_3^2-2{L}_2{L}_3 \cos \left(\pi -{\theta}_3\right) $$

(13.44)

Further simplifying Eq. (13.44) we have the following relation:

$$ {L}_v=\sqrt{L_2^2+{L}_3^2+2{L}_2{L}_3 \cos {\theta}_3} $$

(13.45)

Also, ${\zeta}_1+{\zeta}_2+\left(\pi -{\theta}_3\right)=\pi$

Thus, ${\zeta}_1+{\zeta}_2={\theta}_3$

Assuming that the real links have equal lengths, we have ${\zeta}_1={\zeta}_2$. So,

$$ {\zeta}_1={\zeta}_2={\theta}_3/2 $$

(13.46)

Thus the virtual manipulator link length can be expressed as given in Eq. (13.45) and its joint angle can be expressed as $\left({\zeta}_1+{\theta}_2\right)$ with respect to link of length L ₁. The tip position of the virtual manipulator in X and Y directions can be expressed as,

$$ {X}_{\mathrm{tip}\mbox{\_}{\mathrm{v}}\mathrm{i}\mathrm{r}}={L}_1 \cos {\theta}_1+{L}_v \cos \left({\theta}_1+{\theta}_2+{\zeta}_1\right) $$

(13.47)

$$ {Y}_{\mathrm{tip}\mbox{\_}{\mathrm{v}}\mathrm{i}\mathrm{r}}={L}_1 \sin {\theta}_1+{L}_v sin\left({\theta}_1+{\theta}_2+{\zeta}_1\right) $$

(13.48)

The corresponding tip velocities can be obtained by differentiating the above equations and can be expressed as,

$$ {\overset{.}{X}}_{\mathrm{tip}\mbox{\_}{\mathrm{v}}\mathrm{i}\mathrm{r}}=-{L}_1{\overset{.}{\theta}}_1 \sin {\theta}_1-{L}_v\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\zeta}}_1\right) sin\left({\theta}_1+{\theta}_2+{\zeta}_1\right) $$

(13.49)

$$ {\overset{.}{Y}}_{\mathrm{tip}\mbox{\_}{\mathrm{v}}\mathrm{i}\mathrm{r}}={L}_1{\overset{.}{\theta}}_1 \cos {\theta}_1+{L}_v\left({\overset{.}{\theta}}_1+{\overset{.}{\theta}}_2+{\overset{.}{\zeta}}_1\right) cos\left({\theta}_1+{\theta}_2+{\zeta}_1\right) $$

(13.50)

From Eqs. (13.49) and (13.50) different transformer moduli required for developing the controller based on bond graph for a 3-link planar manipulator can be derived as shown in Fig. 13.27. The transformer moduli obtained from the above mentioned equations are given in Table 13.4.

Fig. 13.27

Bond graph model of virtual link based controller for 3-link planar manipulator

These transformer moduli have been used to construct the bond graph model of the virtual link based controller. The joints of virtual link based controller are represented by ${1}_{{\overset{.}{\zeta}}_1},{1}_{{\overset{.}{\zeta}}_2}$ and ${1}_{{\overset{.}{\zeta}}_1}$ junction. The difference in the reference and actual velocity of the manipulator is termed as the error. The error signal is sent to the proportional-integral (PI) controller and it minimizes the error and sends it to the virtual link based controller. The controller receives the signal from the PI controller and supplies the required amount of torque to the 3-link planar hyper-redundant manipulator to obtain the desired tip trajectory. The modulus value at the ${1}_{{\overset{.}{\zeta}}_1}$ junction has been taken as 0.5 as per the relation obtained in Eq. (13.46).

The virtual link based controller is then modelled with dynamic equivalence along with the plant to simulate different trajectories. The dynamic equivalence is based on the following three conditions according to [14].

• 1. The total mass of the real links must be equal to that of the virtual link

• 2. The centre of gravity (CG) of equivalent system must coincide with that of the virtual link

• 3. Total moment of inertia about an axis through the CG must be equal to that of the virtual link .

The bond graph model of the dynamically equivalent virtual link based controller along with the three link planar manipulator is shown in Fig. 13.28 and the translational dynamics of each link segment in X and Y direction is shown on the left-hand side of the model. Each link has the CG velocity which depends on the link inertia. Therefore in the bond graph model I elements are attached at each velocity junction representing the CG velocity of each link. The transformer moduli obtained from the kinematic analysis with the help of the D-H parameters shown in Table 13.5 have been used to draw the bond graph and they are listed in Table 13.6. Soft pads are artificial compliances which have been used in bond graph to avoid differential causality.

Table 13.5 D-H parameters of 3-link planar manipulator

Table 13.6 Transformer moduli for the bond graph model of the 3-link planar manipulator

Fig. 13.28

Bond graph model of 3-link planar manipulator

The tip velocities are indicated on the top of the bond graph model. Integrators are used to integrate the actual velocity components to obtain the tip positions in X and Y directions, i.e., X _{tip_act} and Y _{tip_act}. The reference position of the end-effector tip is denoted by X _{tip_ref} and Y _{tip_ref}. In Fig. 13.28, J ₁, J ₂, and J ₃ represent the joint inertia, respectively, and the R element on each joint represents the joint resistance.

7.2 Simulation and Animation Results

Three types of end-effector trajectories have been considered to validate the control strategy. First trajectory is a circle, the second one is a knot-shaped trajectory based on Lissajous curve [15], and the third one is a cubic polynomial trajectory. The initial configuration of the 3-link planar manipulator is shown in Fig. 13.29. All the joints were kept at an initial position of 0.698 rad. Parameters used in the simulation are enlisted in Table 13.7. The three different cases considered to check the efficiency of the proposed control scheme are discussed in this section.

Fig. 13.29

Initial configuration of three link planar manipulator

Table 13.7 Simulation parameters for the three different trajectories

7.2.1 Case I: Circular Reference Trajectory

Let us consider the reference trajectory for the 3-link planar manipulator tip in X and Y direction to be a circle of radius A. Then the tip coordinates are

$$ {X}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=A \cos \left(\omega t\right)+{X}_0 $$

(13.51)

$$ {Y}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=A \sin \left(\omega t\right)+{Y}_0 $$

(13.52)

where (X ₀, Y ₀) is the centre of the reference circular trajectory, A is the amplitude of circle, ω is the angular velocity, and t is the time in seconds. Hence the corresponding reference velocity trajectory can be expressed as,

$$ {\overset{.}{X}}_{\mathrm{tip}\_{\mathrm{r}}\mathrm{e}\mathrm{f}}=-A\omega \sin \left(\omega t\right) $$

(13.53)

$$ {\overset{.}{Y}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=A\omega \cos \left(\omega t\right) $$

(13.54)

Figure 13.30 shows a comparative trajectory plot between the reference and actual tip trajectory, and Fig. 13.31 indicates the X _tip and Y _tip positional error with respect to time for the circular trajectory for which simulation was run for 6 s.

Fig. 13.30

Reference versus actual tip trajectory for a circular trajectory

Fig. 13.31

X _tip and Y _tip error versus time for circular trajectory

The X _tip error was recorded between 0.0214 and −0.00912 mm, and the Y _tip error was between 0.043015 and −0.0048 mm. Figure 13.32 shows the animation result for the circular trajectory for the case I. The direction of rotation of the tip position is indicated in this figure.

Fig. 13.32

Animation result for circular trajectory

7.2.2 Case II: Knot Shape Lissajous Curve as Reference Trajectory

Let us assume that the reference trajectory in X and Y directions is a knot-shape trajectory based on the Lissajous curve [15] and the tip coordinates are as follows:

$$ {X}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=A \sin \left( at+\delta \right) $$

(13.55)

$$ {Y}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=B \sin (bt) $$

(13.56)

where A and B are constants and a/b is a ratio to be maintained, i.e., $a/b=0.5$ for this particular knot-shape, and the value of phase difference δ is taken as π/2. The corresponding reference velocity trajectory can be expressed as,

$$ {\overset{.}{X}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}= Aa \cos (at) $$

(13.57)

$$ {\overset{.}{Y}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=Bb \cos (bt) $$

(13.58)

Figure 13.33 shows the comparative tip trajectory between the reference and actual tip trajectory. Figure 13.34 indicates X _tip and Y _tip positional error with respect to time, respectively. For the knot-shape Lissajous curve trajectory the simulation period was 6.4 s. The X _tip error was recorded between 0 and 0.027325 mm and the Y _tip error was between 0 and 0.058107 mm. The animation result for the knot-shape Lissajous curve trajectory along with the tip direction of motion is shown in Fig. 13.35.

Fig. 13.33

Reference versus actual tip trajectory for the knot-shape Lissajous curve

Fig. 13.34

X _tip and Y _tip error versus time for knot-shape Lissajous curve

Fig. 13.35

Animation result for a knot-shaped Lissajous curve trajectory

7.2.3 Case III: Cubic Polynomial as Reference Trajectory

In the third case a cubic polynomial trajectory has been considered. Let us assume the reference trajectory in X and Y directions as follows:

$$ {X}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}={X}_0+3\left({X}_f-{X}_0\right){\left(t/{t}_f\right)}^2-2\left({X}_f-{X}_0\right){\left(t/{t}_f\right)}^3 $$

(13.59)

$$ {Y}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}={Y}_0+3\left({Y}_f-{Y}_0\right){\left(t/{t}_f\right)}^2-2\left({Y}_f-{Y}_0\right){\left(t/{t}_f\right)}^3 $$

(13.60)

where (X _f , X ₀) are the final and initial tip coordinates in X direction and (Y _f , Y ₀) are the final and initial tip coordinates in Y direction, t _f is the final time in sec and t is the simulation time in sec. The corresponding reference velocities can be represented as,

$$ {\overset{.}{X}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=6/{t}_f^2\left({X}_f-{X}_0\right)\left(1-t/{t}_f\right)t $$

(13.61)

$$ {\overset{.}{Y}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=6/{t}_f^2\left({Y}_f-{Y}_0\right)\left(1-t/{t}_f\right)t $$

(13.62)

Figure 13.36 shows the comparative tip trajectory between the reference and actual tip trajectory. Figure 13.37 indicates X _tip and Y _tip positional error with respect to time. For the cubic polynomial trajectory the simulation period was 1.5 s. The X _tip error was recorded between 0.00196 and −0.0069 mm and the Y _tip error was between 0.029 and −0.0002 mm. Figure 13.38 shows the animation result and the tip direction of motion for the polynomial trajectory.

Fig. 13.36

Reference versus actual tip trajectory for cubic polynomial trajectory

Fig. 13.37

X _tip and Y _tip error versus time for cubic polynomial trajectory

Fig. 13.38

Animation for a cubic polynomial trajectory

The animation frames clearly indicate the positions taken by the manipulator links while tracking the desired trajectory. These animations have been performed from simulation data in Symbols Shakti [12], environment.

7.3 Virtual Link Based Controller for Developed In-Vivo Robot

The competence of the virtual link based controller was tested in three-dimensional Cartesian coordinate after getting satisfactory result from the planar case of a 3-link manipulator. As the bond graph model of the actual in-vivo robot has already explained earlier. In this section only the bond graph model of the virtual link based controller is explained. The bond graph model is shown in Fig. 13.39.

Fig. 13.39

Bond graph model of virtual link based controller of in-vivo robot for three-dimensional case

The virtual link lengths of the virtual link based controller used in case of three-dimensional trajectory control of in-vivo robot are derived following the same methodology adopted for the 3-link planar manipulator and can be expressed as:

$$ {L}_{v_1}=\sqrt{L_1^2+{L}_2^2+\left(2{L}_1{L}_2 \cos {\theta}_2\right)},\;\mathrm{and} $$

$$ {L}_{v_2}=\sqrt{L_3^2+{L}_4^2+\left(2{L}_3{L}_4 \cos {\theta}_4\right)} $$

The transformer moduli are obtained using the same methodology that has been adopted in the planar case and they are enlisted in Table 13.8.

Table 13.8 Transformer moduli for the bond graph model of virtual link based controller for three-dimensional case

7.3.1 Simulation Results

The competence of the developed controller was further tested for three-dimensional workspace trajectory for which a cubic polynomial trajectory was chosen. Let us assume the reference trajectory in X and Y directions as follows:

$$ {X}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}={X}_0+3\left({X}_f-{X}_0\right){\left(t/{t}_f\right)}^2-2\left({X}_f-{X}_0\right){\left(t/{t}_f\right)}^3 $$

(13.63)

$$ {Y}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}={Y}_0+3\left({Y}_f-{Y}_0\right){\left(t/{t}_f\right)}^2-2\left({Y}_f-{Y}_0\right){\left(t/{t}_f\right)}^3 $$

(13.64)

$$ {Z}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}={Z}_0+3\left({Z}_f-{Z}_0\right){\left(t/{t}_f\right)}^2-2\left({Z}_f-{Z}_0\right){\left(t/{t}_f\right)}^3 $$

(13.65)

where (X _f , X ₀) are the final and initial tip coordinates in X direction and (Y _f , Y ₀) are the final and initial tip coordinates in Y direction, (Z _f , Z ₀) are the final and initial tip coordinates in Z direction, t _f is the final time in sec, and t is the simulation time in sec. The corresponding reference velocities can be represented as:

$$ {\overset{.}{X}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=6/{t}_f^2\left({X}_f-{X}_0\right)\left(1-t/{t}_f\right)t $$

(13.66)

$$ {\overset{.}{Y}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=6/{t}_f^2\left({Y}_f-{Y}_0\right)\left(1-t/{t}_f\right)t $$

(13.67)

$$ {\overset{.}{Z}}_{\mathrm{tip}\mbox{\_}{\mathrm{r}}\mathrm{e}\mathrm{f}}=6/{t}_f^2\left({Z}_f-{Z}_0\right)\left(1-t/{t}_f\right)t $$

(13.68)

The simulation parameters are expressed in Table 13.9.

Table 13.9 Simulation parameters for three-dimensional case

The reference and actual tip trajectory is shown in Fig. 13.40 from which it can be seen that the robot tip precisely followed the reference path assigned to it from one known position to another in the three-dimensional Cartesian coordinate.

Fig. 13.40

Reference versus actual tip trajectory for three-dimensional cubic polynomial trajectory in workspace

8 Concluding Remarks

In this chapter a forward kinematic analysis of the in-vivo robot manipulator was performed. The results obtained from the simulation validate the bond graph model. Further an inverse kinematic solution was found to determine the pulley rotation for different known position of the tip by taking three different trajectories. In fact this is the actual condition faced by surgeons where they would be interested in taking the robot tip from one position to the desired position. This inverse kinematics solution was validated using simulation results. Further experiments have been carried out to validate the results obtained in simulation and comparisons between simulated and experimental results are satisfactory. A very small deviation between simulated and experimental results of X and Z coordinates for tip position in three different cases were observed.