Open access peer-reviewed chapter

Micro-Robot Management

By Wael A. Al-Tabey

Submitted: December 8th 2011Reviewed: February 24th 2012Published: September 26th 2012

DOI: 10.5772/39240

Downloaded: 3530

1. Introduction

Micro-Robots have been and continue to be essential components of medical field. Micro-Robots are used in medical field to surgical applications. Knowledge of the kinematics, dynamics and trajectory planning of these Micro-Robots is most important for their design and control. MATLAB is a modern tool that has transformed the mathematical calculations methods because MATLAB not only provides numerical calculations but also facilitates analytical calculations using the computer. The present textbook uses MATLAB as a tool to solve problems from micro-robots. The intent is to show the convenience of MATLAB for micro-robots analysis. Using example problems the MATLAB syntax will be demonstrated. MATLAB is very useful in the process of deriving solutions for any problem in micro-robots. The chapter includes a most problem of micro-robots for surgical applications that are being solved using MATLAB. The programs are available at the end of this chapter.

Robots are widely used in medical field for getting minimally invasive surgery efficiently and accurately. Minimally invasive surgery is an innovative approach that allows reducing patient trauma, postoperative pain and recovery time [1]. The kinematic and dynamic analysis of the robot, for any applications whether in medical field or another, are very important. The direct aim is to properly select the workspace and the actuator size. Frumento [1] designed a minimally invasive robot for heart surgery. They concentrated mainly on the kinematic analysis and the workspace of the robot. Tsai and Hsu [3] investigated a parallel surgical robot having six degrees of freedom. They studied the kinematics only, to obtain the workspace of surgical robot and control it using Fuzzy Logic control. Miller and Christensen [4] analyzed the dynamics of the Multi-rigid-body robot using Newton’s second law of motion and used the results to design the controller. Featherstone and Orin [5] investigated the robot dynamics and used Newton-Euler technique to obtain the equation and algorithms of robot motion. Wang, et al [6] designed the dimensional synthesis of 6-DOF Micro-surgery manipulator (Micro Hand).They studied the kinematics and the workspace of the manipulator they considered by an Optimal design method. Alici and Shirinzadeh [7] obtained the singularity loci of parallel manipulators exemplified by a 3-DOF spherical parallel manipulator. They utilized the inverse kinematics, and Jacobian matrices to obtain the velocity equation of the actuator and the end effector. The determinants of the manipulator Jacobian matrices were evaluated for a specified set of geometric parameters. Finally, the singularity loci of the manipulator in Cartesian space were generated. Ben-Horin, et al [8] analyzed the kinematics and dynamics of parallel robot consisting of three plenary actuated links [6-DOF parallel manipulator]. They investigated by direct kinematic analysis and obtained the dynamics equations and algorithms using Newton’s second law and presented the system performance. Bonnifait and Garcia [9] studied the 6-DOF dynamic localization of an outdoor mobile robot. They obtained the robot dynamics equations and algorithms using the numerical method (Taylor method) and used the azimuth and elevation angles of known landmarks, collected by a rotating linear camera to obtain the simulation results of the robot. They finally presented and analyzed the results of real experiments, performed with an outdoor mobile robot. Abdellatif and Heimann [10] investigated the inverse dynamics equations of 6-DOF fully parallel manipulators using the Lagrangian formalism. With respect to a proposed set of generalized coordinates and velocities they obtained the final form with respect to the robot’s active coordinates. Attention was paid to the transformation of the sub chains dynamics. Finally, a systematic study of the resulting computational effort was presented and discussed with respect to results of other methods and approaches of other researches. Zhu, et al [11] investigated the kinematic and dynamic modeling for a newly developed parallel robot with the proposed Tau configuration and used the inverse kinematics method to obtain the kinematic equations of end effector of 3-DOF parallel robot. The dynamic modeling of 3-DOF parallel robot was derived by analytical solutions, which were verified by both numerical simulation and actual experiments. This analytical approach enabled the real-time control of this parallel robot with high positioning accuracy. Gouliaev and Zavrazhina [12] investigated the dynamic and kinematic control of the spatial movements of a flexible multi-link manipulator. They focused on the dynamics of a flexible multi-link manipulator by Euler-Bernoulli method, so that each element was in a compound motion. A technique for the numerical construction of solutions for an essentially non-linear hybrid-type system of constituent equations was proposed. They used the linear control method. Eliodoro and Serna [13] investigated the inverse dynamics of flexible robots. They focused on the new and general technique for solving the inverse dynamics of flexible robots. The proposed method finds the joint torques that must be applied by the actuators to obtain a specified end-effector trajectory. The inverse dynamics of flexible robots are derived by using the Euler-Bernoulli beam theory and Lagrange's equations. The finite element method was utilized to discretize space variables. They finally established the global dynamic equations of the robot. Kinematic constraints were introduced in the dynamic equations by means of a penalty formulation. The system performance was drawn for different tip trajectories.

Martins et al [14] examined an adaptive controller to guide a unicycle-like mobile robot during trajectory tracking. They concentrated, mainly on the kinematic analysis to design the adaptive controller using Lyapunov theory. The kinematic equations of mobile robot were found by inverse kinematics method to obtain the desired values of the linear and angular velocities, which represented the input to adaptive controller. Valero et al [15] investigated a trajectory planner for 3-DOF industrial robots that have to operate in workspaces with obstacles. They found the workspace modeling analytically using the differential equations of all joints angle and solved them in Cartesian coordinates system. The trajectory planner for 3-DOF industrial robots they found by workspace model using the finite element method for joints and end effector. Integrating all elements, the function of the trajectory planner for 3-DOF industrial robots was obtained. Finally they minimized the objective function of the trajectory planner to attain the minimum time. Geng [16] investigated the dynamics and trajectory planning of a planar flipping robot with two feet and one-leg robot. Geng made a dynamic analysis to obtain the trajectory planning using Lagrange’s formulation. Geng presented the simulation results for joints kinetic variables with time. Alessandro and Vanni [17] investigated a technique for optimal trajectory planning of robot manipulators to minimize the jerk. They concentrated on the trajectory planning of robot manipulators. In order to get the optimal trajectory, an objective function composed of two terms was minimized. The first term was proportional to the total execution time, and the second was proportional to the integral of the squared jerk. Finally, they minimized the time of trajectory planning to minimize energy (or actuator effort) and jerk of manipulator joints. They represented the Simulation results for joints kinetic variables and jerk against the minimized time. Pires et al [18] tested a manipulator trajectory planning with multiple objectives and obstacle avoidance (MOEA).It is a non-trivial optimization problem. They concentrated on the trajectory planning of 2-DOF robot manipulators using MOEA method and simulated for several ranges of joint angle. Finally they gave the simulation results for joints kinetic variables with minimized time. Chettibi et al [19] studied the problem of minimum cost trajectory planning for 2-DOF industrial manipulators. They studied the optimal control via direct parameter optimization of joint positions then they concentrated on the trajectory planning of 2-DOF industrial manipulators for six ranges of joint angle. Finally, they represented the simulation results for joints kinetic variables, jerk and torque with minimized time, to obtain the optimum parameters of joint positions and minimum cost trajectory planning. Alessandro and Vanni [20] investigated a method for smooth trajectory planning of robot manipulators. In order to ensure that the resulting trajectory is smooth enough they used the same method used by them in reference. They also presented in their work a new method to obtain the smooth jerk by composing the overall trajectory with respect to other trajectory optimization techniques.

1.1. Introductory remarks on robots for medical field applications

Robots in medicine are recent entry, beginning as generic instrumental aides and aiming at specialized duties once technology sophistication enables effective settings. Several classifications are used, mainly, dressing taxonomy by means of the expected accomplishments [1]:

  1. Patients and disabled aid: bed automation, walking assistants, delivery servants, etc.

  2. Laboratory support: clinical testers, radiation therapies assistants, etc.

  3. Soundness care: pace-makers, health-monitors, drugs dosing up suppliers, etc.

  4. Surgery help: surgeon’s servants, remote effectors, autonomous actors, etc.

Moreover, roughly speaking, the example taxonomy distinguishes extra corporeal fixtures, mainly derived out of conventional technologies, from in-body active devices, generally requiring invasive actions, thus, critically dependent on micro-mechanics and nanotechnologies. With focus on surgery robots, four kinds of tasks are, generally, considered:

  1. Organ inspection: cerebral probing, laparoscopic monitoring, etc.

  2. Organ nursing or repair: internal anastomosis, obstruction relief, etc.

  3. Organ removal: cysts excision, lymph node dissection, etc.

  4. (Artificial) organ implant: prosthesis insertion, etc.

Surgical robotic systems are commonly classified according to the degree of direct control the surgeon has over the machine [2].Under this classification there are three principal types of robots Table 1:

  1. Autonomous: performs a preoperative plan without any immediate control from the surgeon.

  2. Surgical Assist Device: surgeon and robot share control.

  3. Teleoperator: function of the robot completely controlled by the surgeon.

Type of systemExamplesFunction(s)Clinical Discipline(s)
AutonomousRobodocPercutaneous renal needle placementOrthopedic surgery
PAKY-RCMProstatectomyUrology
ProBotHip surgeryUrology
Surgical AssistCasparVoice controlled telescopeOrthopedic surgery
AESOPStereotacticMultiple
NeuroMateNeurosurgeryNeurosurgery
TeleoperatorAcrobatKnee arthroplastyOrthopedic surgery
PUMA (Programmable Universal Machine
for Assembly)
MultipleMultiple
Da VinciMultipleMultiple
ZeusMultipleMultiple
NeurobotMultipleNeurosurgery

Table 1.

Classification of robotic surgical systems

The systems in study at this present work are Zeus and Da Vinci robotic arms which are usually attached to a patient-side tower structure and consists of two to three arms that control the operative instruments and a separate arm that controls the video endoscope as shown in the Figure 1.

Figure 1.

Zeus and Da Vinci robotic arms

Both the Zeus and Da Vinci systems enhance dexterity in several ways. Internal software filters out the natural tremor of a surgeon’s hand, which becomes particularly evident under high magnification and problematic when attempting fine maneuvers in very small fields. In addition, the system can scale movements such that large movements of the control grips can be transformed into smaller movements inside the patient. Finally the system group has 7-DOF. The Surgery arm has 6-DOF plus 1-DOF for the tool actuation. Arrangement, the DOF ‘‘a’’ and ‘‘b’’ are respectively the last DOF at the carrier and the 6-DOF robot joints has Roll-Yaw-Pitch motions as shown in the Figures.2, 3 and 4.

Figure 2.

The ZEUS® surgery tools

Figure 3.

Da Vinci® surgery tools

Figure 4.

Roll-Yaw-Pitch motions

2. The kinematic model

2.1. The forward kinematics

The forward kinematics problem is concerned with the relationship between the individual joints of the robot manipulator and the position and orientation of the tool or end effector. Stated more formally, the forward kinematics problem is to determine the position and orientation of the end effector, given the values for the joint variables of the robot. The joint variables are the angles between the links in the case of revolute or rotational joints, and the link extension in the case of prismatic or sliding joints.

The solution is always unique: one given joint position vector always corresponds to only one single end effector pose. The FK problem is not difficult to solve, even for a completely arbitrary kinematic structure.

There are different methods for a forward kinematic analysis: like using straightforward geometry and using transformation matrices.

2.1.1. The Denavit-Hartenberg convention

The D-H modeling rules:

  1. Find and identify all joint axes: Z0 to Zn-1.

  2. Establish the base frame. Set base origin anywhere on the Z0 axis. Choose X0 and Y0 conveniently and to form a right hand frame.

  3. Locate the origin Oi where the common normal to Zi-1 and Zi intersects Zi.

If Zi intersects Zi-1 locate Oi at this intersection.

If Zi-1 and Zi are parallel, locate Oi at Joint (i+1).

  1. Establish Xi along the common normal between Zi-1 and Zi through Oi, or in the direction normal to the plane Zi-1 - Zi if these axes intersect. See Figure 5.

  2. Establish Yi to form a right hand system

  3. Repeat Steps 3 to 5 for i= 1 : n-1.

  4. Establish the end effector (n) frame: OnXnYnZn. Assuming the nth joint is revolute.

Set Kn = a along the direction Zn-1 and establish the origin On conveniently along Zn, at center of tool tip. Set jn=o in the direction of tool closure (opening) and set in= n, such that n=oxa.

If the tool is not a simple gripper, set Xn and Yn conveniently to form a right hand frame.

  1. Create a table of “Link” parameters: See Figure 5.

Joint Angle i : angle between Xi-1 and Xi about Zi.

Link Offset di : distance from Xi-1 and Xi along Zi.

Link Twist i : angle between Zi and Zi+1 about Xi.

Link length ai : distance between Zi and Zi+1 along Xi.

  1. Form HTM matrices A1, A2 … An from the information contained in each row of the LP table by substituting , d, and a into the general model.

  2. Build forward kinematic solution: = A1* A2 * … * An.

Figure 5.

Construction of the link frame

2.2. The kinematics of Zeus and Da Vinci robotic arms

The geometrical model of the surgical robot in this study has 6 degrees of freedom (DOF) and an extra one for tool action. The end-effector has 3 rotations (Roll, Pitch and Yaw) as shown in Figure 6 and the frame assignment of 6-DOF surgical manipulator is represented in Figure 7.

Figure 6.

The geometrical model of surgical manipulator

Figure 7.

The frame assignment of 6-DOF surgical manipulator

The 6-DOF manipulator kinematic parameters are derived using Denavit Hartemberg formulation shown in Table 2.

Link #θjdjajαjT
1θ10L100T1
2θ20L2901T2
3θ300-902T3
4θ4L30903T4
5θ500-904T5
6θ6L4005T6

Table 2.

Full mobility robotic tool: geometry and link parameters

The flowchart representing the sequence of generating the MATLAB CODE of the link transformations matrix is shown in Figure 8.

Figure 8.

The flowchart of the link transformations matrix

The general form of the Homogeneous Transformation Matrix is:

Tj-1j=[cosθjsinθjcosαjsinθjsinαjajcosθjsinθjcosθjcosαjcosθjsinαjajsinθj0sinαjcosαjdj0001]E1

The link transformations matrix can be given as:

0T1=[C1S10L1C1S1C10L1S100100001]1T2=[C20S2L2C2S20C2L2S201000001]2T3=[C30S30S30C3001000001]3T4=[C40S40S40C40010L30001]4T5=[C50S50S50C5001000001]5T6=[C6S600S6C600001L40001]

The kinematics equations of the end effectors are manipulated using MATLAB symbolic Toolbox were as follows:

[PXPYPZZ]=[((-(C1*C2-S1*S2)*C3*C4-(-C1*S2-S1*C2)*S4)*S5+(C1*C2-S1*S2)*S3*C5)*L4-(C1*C2-S1*S2)*S3*L3+C1*L2*C2-S1*L2*S2+L1*C1((-(C1*S2+S1*C2)*C3*C4-(C1*C2-S1*S2)*S4)*S5+(C1*S2+S1*C2)*S3*C5)*L4-(C1*S2+S1*C2)*S3*L3+S1*L2*C2+C1*L2*S2+L1*S1(-S3*C4*S5-C3*C5)*L4+C3*L31]

2.3. The robot workspace

The workspace of a robot can be defined as the set of points that are reachable by the manipulator (with fixed base). Roughly speaking the workspace is the volume of space which the end effector of the robot can reach. Both shape and total volume are important. Workspace is also called work volume or work envelope.

The workspace depends on the characteristics of the manipulator; physical configurations, size, number of axes, the robot mounted position (overhead gantry, wall-mounted, floor mounted, on tracks, etc), limits of arm and joint configurations. The addition of an end effector can move or offset the entire work volume.

The kinematics design of a manipulator can tailor the workspace to some extent to the operational requirements of the robot.

Some robots will have unusable spaces such as dead zones, singular poses, and wrist-wrap poses inside of the boundaries of their reach. Elbow manipulators tend to have a wider volume of workspace.

  1. Dexterous workspace: This is the volume of space which the end-effector of the manipulator can reach with all orientations.

  2. Reachable workspace: This is the volume of space which the end-effector of the manipulator can reach with at least one orientation.

The dexterous workspace is obviously a subset of the reachable workspace.

2.3.1. The workspace calculation

The workspace may be found mathematically by writing equations that define the robot's links and joints and including their limitations, such as ranges of motions for each joint. Alternatively, the workspace may be found empirically, by moving each joint through each range of motion and combining all the space in can reach and subtracting what it cannot reach.

The workspace of the surgical manipulator can be represented by solving the inverse kinematic equations and taking into consideration all the physical limits of the joints. Figure 9 represents the flowchart showing the sequence of generating the three dimensional workspace of the robot and end-effector to be manipulated using the MATLAB symbolic Toolbox.

Figure 9.

The flowchart of The Robot workspace

Table 3 represent the physical limits of the six joints while Figure10 represent the workspace of end-effector respectively.

Link #123456
θi (degree)-180°-90º0-180°-90º-180°
θf (degree)180°90º180°180°90º180°
L (mm)0501536039.5

Table 3.

Joint coordinates ranges

The workspace of the end-effector depends on the physical limits of the six joints angle, i.e. if the angle range of the robot joints is changes the workspace of the robot changes. So it is important to take into account the accuracy in determining the angle range of the robot joints to get the required workspace which covers the work area.

Figure 10.

The work space of end effector

2.4. The robot Jacobian

The Jacobian is a representation of the geometry of the elements of mechanism in time. It allows the conversion of differential motions or velocities of individual joints to differential motions or velocities of pints of interest. It also relates the individual joint motion to overall mechanism motions. Jacobian is time related; since the values of θivary in time, and the magnitude of the elements of Jacobian vary in time as well.

2.4.1. The differential motions and velocities equations

Differential motions are small movements of robot parts that can be used to derive velocity relationships between different parts of the robot. To find these relations the following steps, are to be considered:

  1. Frames relative to a fixed frame.

  2. Robot joint relative to a fixed frame.

  3. Jacobian matrix.

  4. Robot velocity relationship.

2.4.2. The Jacobian equations

Suppose we have a set of equations Yi in terms of variables Xj

Yi=fi(X1,X2,X3.,Xj)E2

The differential change in Yi for a differential change in Xj is:

δY1=f1X1δX1+f1X2δX2++f1XiδXjδY2=f2X1δX1+f2X2δX2++f2XiδXjδYi=fiX1δX1+fiX2δX2++fiXiδXjE3

2.4.3. The Jacobian matrix

[δYi]=[fiXi][δXj][D]=[J][Dθ]E4

2.4.4. The Jacobian relations

The Kinematics equations of the end effectors which are used to calculate the Jacobian matrix of the robot are:

PX=f1=((-(C1*C2-S1*S2)*C3*C4-(-C1*S2-S1*C2)*S4)*S5+(C1*C2-S1*S2)*S3*C5)*L4-         (C1*C2-S1*S2)*S3*L3+C1*L2*C2-S1*L2*S2+L1*C1

PY=f2=((-(C1*S2+S1*C2)*C3*C4-(C1*C2-S1*S2)*S4)*S5+(C1*S2+S1*C2)*S3*C5)*L4-         (C1*S2+S1*C2)*S3*L3+S1*L2*C2+C1*L2*S2+L1*S1

PZ=f3=(-S3*C4*S5-C3*C5)*L4+C3*L3

Figure 11.

The flowchart of the Jacobian matrix

The Jacobian matrix of the robot can be calculated by MATLAB symbolic Toolbox using the kinematics equations as shown in the Figure 11.

The differential equations of motion of the end effector for the surgical robot are represented by:

dPx =Vx=((((C1*S2+S1*C2)*C3*C4-(-C1*C2+S1*S2)*S4)*S5+(-C1*S2-S1*C2)*S3*C5*L4-(-C1*S2-S1*C2)*S3*L3-S1*L2*C2-C1*L2*S2-L1*S1)*dθ1+((((C1*S2+S1*C2)*C3*C4-(-C1*C2+S1*S2)*S4)*S5+(-C1*S2-S1*C2)*S3*C5)*L4-(-C1*S2-S1*C2)*S3*L3-S1*L2*C2-C1*L2*S2)*dθ2+(((C1*C2-S1*S2)*S3*C4*S5+(C1*C2-S1*S2)*C3*C5)*L4-(C1*C2-S1*S2)*C3*L3)*dθ3+((C1*C2-S1*S2)*C3*S4-(-C1*S2-S1*C2)*C4)*S5*L4*dθ4+(((-C1*C2+S1*S2)*C3*C4-(-C1*S2-S1*C2)*S4)*C5-(C1*C2-S1*S2)*S3*S5)*L4*dθ5

dPy =Vy=((((-C1*C2+S1*S2)*C3*C4-(-C1*S2-S1*C2)*S4)*S5+(C1*C2-S1*S2)*S3*C5)*L4-(C1*C2-S1*S2)*S3*L3+C1*L2*C2-S1*L2*S2+L1*C1)*dθ1+((((-C1*C2+S1*S2)*C3*C4-(-C1*S2-S1*C2)*S4)*S5+(C1*C2-S1*S2)*S3*C5)*L4-(C1*C2-S1*S2)*S3*L3+C1*L2*C2-S1*L2*S2)*dθ2+(((C1*S2+S1*C2)*S3*C4*S5+(C1*S2+S1*C2)*C3*C5)*L4-(C1*S2+S1*C2)*C3*L3)*dθ3+((C1*S2+S1*C2)*C3*S4-(C1*C2-S1*S2)*C4)*S5*L4*dθ4+(((-C1*S2-S1*C2)*C3*C4-(C1*C2-S1*S2)*S4)*C5-(C1*S2+S1*C2)*S3*S5)*L4*dθ5

dP=Vz=((-C3*C4*S5+S3*C5)*L4-S3*L3)*dθ3+S3*S4*S5*L4*dθ4+(-S3*C4*C5+C3*S5)*L4*dθ5

Figure 12.

The relation between angle of joints and the end effector differential translation

The previous differential equations of motion of the end effector represent the relation between the magnitude of the elements of Jacobian (the elements of end effector motion) and the joints angle. Figures 12 a, b, c, d, e and f represents the relation between angle of joints and the end effector differential translation. This shows six figures is (a, b, c, d, e and f) for six joints. It is to be noted that in Figures 12-a and 12-b for the first and second joints respectively the relation between angle of joint and the end effector differential translation in the Z direction is constant i.e. the motion of the end effector in the Z direction is not affected by the change in angle of the first and second joints. In the same time the motion of the end effector in all directions (X, Y and Z) is not affected by the change in angle of the six joints because the frame of the end effector is in the same directions of the frame of the joint number six. This can clearly be inferred from the geometrical model and the frame of the robot given in Figures 6 and 7 respectively.

The relations between the magnitude of the elements of Jacobian (the elements of end effector motion at all directions) i.e. dPX, dPY and dPZ give of the path of the robot joints as shown in Figures 13 a, b, c, d, e and f.

Figure 13.

The path of the robot joints

3. The trajectory planning

3.1. Introduction to the trajectory planning

Robot study is divided into two parts; they are the kinematics and dynamics. This means that using the equations of motion of the robot, its position can be determined if the joint variables are known. Path and trajectory planning relates the way a robot is moved from one location to another in controlled manner. In this chapter, a study of the sequence of movements is to be made to create a controlled movement between motion segments, in straight-line motion, or in sequential motions. Path and trajectory planning requires the use of both kinematics and dynamics of robots. In practice, precise motion requirements are so intensive that approximations are always necessary [23].

What is the different between the path and the trajectory planning?

A Path is defined as a sequence of robot configurations in particular order without regard to timing of these configurations as shown in Figure 14. A trajectory is concerned about when each part of the path must be obtained thus specifying timing (Velocity and Acceleration). Or a trajectory is a spatial position/time curve that usually represents a desired manipulation motion in either link or Cartesian space as shown in Figure 15.

Figure 14.

Sequential robot movements in a path [23]

Figure 15.

Sequential motions of a robot to follow a straight line [23]

3.2. The methods to calculate trajectory planning

There are four different methods which have been derived to calculate the trajectory planning of the robot using MATLAB code. The trajectory planning in the four methods needs two parameters, namely the joint angle range and the final time required to complete the process. The first one was selected from the workspace needed to complete the process In any case, this dependent on the type of surgical applications and can easily be identified. The final time required is a very important parameter to derive the trajectory planning of the robot. As all the results that have been inferred from the orientation, velocity, acceleration and torque of the robot are based on this parameter. If the time increases the velocity required decreases and acceleration i.e. the inertia of the robot link consequently decreases accidentally irrespective of the methods used. The orientation must be seen carefully examined and the behaviour should be increasing gradually with the time from the initial angle to the final angle. The final time required to complete the process for each method should be optimized and this is called optimal planning. In this work the trajectory planning of the four times is derived and then, comparison of the results at specific joint is made. The proper time which gives satisfactory results is obtained.

3.2.1. Third-Order polynomial trajectory planning

The most common techniques for trajectory planning for industrial robots are the use of polynomial of different orders, such as Cubic and B-splines, linear segments with parabolic blends and the soft motion trajectory [22]. The Linear Segments with Parabolic blends trajectories are faster and more suitable for industrial applications. On the other hand, the higher order polynomials trajectory as well as the soft motion trajectory [24] are easy to design and control especially for the jerk. They are accurate, precise and suitable for medical applications. In this work, the trajectory planning for each joint is designed using third order polynomial as a rest-to-rest manoeuvring where the link starts from rest, accelerates and decelerates at the end of the trajectory. The trajectory is given by [23]:

θ(t)=C0+C1t+C2t2+C3t3E5

In whichC0,C,1C2andC3are coefficients to be determined from the initial conditions as follows:

θ˙(t) = C1+ 2 C2 t + 3 C3 t2E6

θ˙(ti) =0  and  θ˙(tf) =0(Rest to Rest manouvering)

By substituting in to equation (6):

θ˙(ti) =C1=

θ˙(tf) = C1+ 2 C2 tf + 3 C3 tf2=0E7

The initial and final location and orientation of robot are known from:

θ(ti) =θi and  θ(tf) =θf

By substituting in to equation (5):

θ(ti) =C0=θi 

θ(tf) = C0+C1tf+C2tf2+C3tf3=θfE8
(8)

By solving equations (7), (8) simultaneously:

C2=3(θfθi)/tf2andC3=2(θfθi)/tf3

3.2.2. Fifth-Order polynomial trajectory planning

The third order trajectory only takes into account starting and end velocities. The equations of the fifth order polynomial takes into account starting and end accelerations. In this case, the total number of boundary conditions are six, allowing a fifth.

The initial and final velocities are zero. (Rest to Rest manoeuvring) and the trajectory is given by [23]:

θ(t)=C0+C1t+C2t2+C3t3+C4t4+C5t5E9

In whichC0,C,1C2,C3,C4andC5are coefficients to be determined from the initial and final conditions as follows:

θ˙(t)=C1+2C2t+3C3t2+4C4t3+5C5t4E10
θ¨(t)=2C2+6C3t+12C4t2+20C5t3E11

θ˙(ti) =0  and  θ˙(tf) =0(Rest to Rest manoeuvring)

θ¨(ti) =θ¨i and θ¨(tf) =θ¨f

C0=θi,C1=θ˙i,C2=θ¨i2,C3=20θf-20θi-(8θ˙f+12θ˙i ) tf-(3θ¨i-θ¨f)tf22tf3,

C4=30θf-30θi+(14θ˙f+16θ˙i ) tf+(3θ¨i-2θ¨f)tf22tf4andC5=12θf-12θi-(6θ˙f+6θ˙i ) tf+(θ¨i-θ¨f)tf22tf5

3.2.3. Linear segments with parabolic blends

Linear segments can be blended with parabolic sections at the beginning and the end of the motion segment, creating continuous position and velocity. Acceleration is constant for the parabolic sections, yielding a linear velocity at the common points A and B as shown in the Figure 16.

3.2.3.1. First parabolic blends (t0 to ta)

θ(t)= C0+ C1t +  12  C2t2E12

θ˙(t)=  C1 +   C2tandθ¨(t)=  C2

Figure 16.

Scheme for linear segments with parabolic blends

The position of the robot at time t0 is known and using the inverse Kinematic equations of the robot, the joint angles at via points and at the end of the motion can be found.

To blend the motion segments together, the boundary conditions at each point are used to calculate the coefficients of the parabolic segments.

3.2.3.2. Straight line (ta to tb)

θA=θB+wt,θ˙A=θ˙B+wandθ¨=0

3.2.3.3. Second parabolic blends ( tftb to tf )

θ(t)= Cfw2tb(tft)2E13

θ˙(t)= wtb(tft)andθ¨(t)= -wtb

tb=(θiθf+wtf)w,C2wtbandwmax=2(θfθi)tf

3.2.4. Soft motion trajectory planning

In this method we consider the trajectory planning of points generated by a motion planning technique. The motion planner calculates the trajectory which the end effector must follow in space. However, the temporal characteristics of this movement are independent. One important difference between industrial robotic manipulators and service robot applications is the human interaction, which introduce safety and comfort constraints. In this work, we define soft motions conditions to facilitate this cohabitation. An on-line trajectory planner is proposed here. It generates the necessary references to produce soft motion and a control loop that guarantees the end effector’s motion characteristics (jerk, acceleration, velocity and position) in the Cartesian space, by using quaternion feedback. Two visual feedback control loops are proposed: a visual servoing control loop in a shared position - vision schema and a visual guided loop (which is the general case of soft motion trajectory) are given by:

3.2.4.1. The motion with a maximum jerk (Jmax)

θ(t)=θ0+θ˙0t+12θ¨t2+16Jmaxt3E14

θ˙(t)=θ˙0+θ¨0t+12Jmaxt2,θ¨(t)=θ¨0+JmaxandJ(t) = Jmax 

3.2.4.2. The motion with a maximum acceleration (Amax)

θ(t)=θ0+θ˙0t+12θ¨maxt2E15

θ˙(t)=θ˙0+θ¨max,θ¨(t)=θ¨maxandJ(t) = 0 

3.2.4.3. Finally, the equations for the motion with a maximum velocity (Vmax)

θ(t)=θ0+θ˙maxtE16

θ˙(t)=θ˙max,θ¨(t)=0andJ(t) = 0 

The initial conditions are:

θ¨(0)=0,θ˙(0)=0,θ(0)=θ0andD=θDθ0

Where: D is general traversed angular movement.

The final conditions are:

θ¨(tf)=0,θ˙(tf)=0andθ(tf)=θD

We have two limit conditions to obtain the traversed angular movement:

  • Condition (1):

Where θ˙maxis reached. It means, θ¨maxis reached too. Then we have to find the traversed angular (Dthr1). The limit times used to calculate (Dthr1) are:

Tj=Tjmax,Ta=TamaxandTv=0

The angular movement (Dthr1) becomes:

Dthr1=θ¨maxθ˙maxJmax+θ˙max2θ¨max

  • Condition (2):

Where only θ¨maxis reached. Then we have to find the traversed angular (Dthr2). The limit times used to calculate (Dthr2) are:

Tj=Tjmax,Ta=0andTv=0

The angular movement (Dthr2) becomes:

Dthr2=2θ¨3maxJ2max,Tjmax=θ¨maxJmaxandTamax=(θ˙maxθ¨max)(θ¨maxJmax)

The soft motion trajectory planning of 6-DOF surgical robot is divided in to three cases depending on the maximum Jerk algorithm as:

  • Case (1) (General case): If D ≥ Dthr1

Tj=Tjmax,Ta=TamaxandTv=DDthr1θ˙max

  • Case (2): If Dthr2 >D ≥ Dthr2

Tv=0,Tj=TjmaxandTa=θ¨2max4Jmax+Dθ¨max3θ¨max2Jmax

  • Case (3): If D < Dthr2

Tv=0,Ta=0andTj=D2Jmax3

3.3. The robot trajectory planning parameters

In this work, four different methods are applied here to design the joints trajectories third order polynomial, fifth order polynomial, linear segments with parabolic blends and soft motion trajectory. The trajectories have the same initial and final angles and different four duration times (5, 10, 20 and 60 s) are applied to choose the best duration time give the correct dynamic response. As well but they differ in the acceleration and the jerk. After designing the joints trajectories, the hub torques of the robot actuators can be simulated using MATLAB. The parameters of six joints were obtained using inverse kinematics analysis as shown in Table 4.

The flowchart representing the sequence of generating the trajectory planning is shown in the Figure 17.

Link#123456
ϖi(degree)-180°-90º0-180°-90º-180°
ϖf(degree)180°90º180°180°90º180°
L (mm)0501536039.5
ϖ̦i( degree /s2)555555
ϖ̦f( degree /s2)-5-5-5-5-5-5
ϖ̥max( degree /s)135707013570135
ϖ̦max( degree /s2)804040804080
Jmax( degree /s3)160808016080160
ti(s)000000
tf(s)555555

Table 4.

Full robotic mobility information used in trajectory planning

Figure 17.

The flowchart of the trajectory planning

Where: Position.1 for first methods of trajectory planning and Position.2 for Soft motion trajectory planning.

4. The dynamic model

4.1. Introduction to the dynamic model of robot

Manipulator dynamics is concerned with the equations of motion, the way in which the manipulator moves in response to torques applied by the actuators, and external forces. The history and mathematics of the dynamics of serial-link manipulators are well covered in the literature. The equations of motion for an n-axis manipulator are given by [25]:

Q=M(q) q¨ + C(q.q˙  ) q˙ + G(q) E17

These equations of motion can be derived by six methods, namely the Newton's second law method, D'Alembert method, Lagrange method, Hemilton method, Lagrange-Euler method and Newton-Euler method. But not all previous methods can be used to derive the equations of motion for the robot subject of this work. That is not all methods can easily derived the equations of motion for a robot having multi-degree of freedom.

4.2. Dynamic equations for multiple-degree of freedom robots

As we have stated, the dynamic equation for two-degree of freedom system is much more complicated than a one-degree of freedom system. Similarly, these equations for multiple-degree of freedom robot are cumbersome and complicated, but can be found by calculating the kinetic and potential energies of the links and joints [25]. For the robot considered in this work the robot has 6-DOF and the most appropriate method to derive the equations of motion is likely to be Lagrange-Euler technique.

4.2.1. Derivation of the equations of motion by Lagrange-Euler technique

The Lagrange-Euler technique is utilized here to calculate the kinetic energy, potential energy and to derive the dynamic equations in symbolic form using the MATLAB symbolic toolbox for the six-degree of freedom robot. The equations of motion are given in a concise form similar to that given in [25].

4.2.1.1. The total kinetic energy

The total kinetic energy of multiple-degree of freedom robot is given in a concise form as:

Ki=12i=1np=1ir=1iTrace(UipJiUirT)q˙pq˙r+121nIiactq˙i2E18

Uip=T0iθP=QPT0i,Uir=T0iθr=QrT0i,Uir=Uri,UiP=UPiand

Qi(revolute)=[0100100000000000],Qi(prismatic)=[0000000000010000]

But in our case all joints in the robot arm used in surgical applications are considered revolute joints whereQP= Qr= Q.

4.2.1.2. The potential energy

The potential energy of multiple-degree of freedom robot may be given in a concise from as:

P=j=1nPi=j=1n[migT(T0jXrj¯)]E19

gT=[gxgygz0]

4.2.1.3. Robot equations of motion

The equations of motion of multiple-degree of freedom robot be given in a compact form as:

Ti=j=1nDijq¨j+Ii(act)q¨i+j=1nk=1nDijkq˙jqk+DiE20

Dij=p=max(i,j)nTrace(UpjJpUpiT),Dijk=p=max(i,j,k)nTrace(UpjkJpUpiT),

Di=p=1nmpgTUpjrp¯,UPik=UPiθk=UPiQkT0k

Ji=[(IXX+IYY+IZZ)2IXYIXZmiXi¯IXY(IXXIYY+IZZ)2IYZmiYi¯IXZIYZ(IXX+IYYIZZ)2miZi¯miXi¯miYi¯miZi¯mi]

Link #123456
Iact (Nmm)100001000010000100001000010000
m (kg)6.0556.0556.0556.0556.0556.055
L (mm)0501536039.5
g (m/s2)9.819.819.819.819.819.81
R (mm)757575757575
Ixx(1/12)*mi*(3*(R2)+(Li/4)2)
Iyy(1/12)*mi*(3*(R2)+(Li/4)2)
Izzmi * R2
Ixy0
Ixz0
Iyz0
XLi/2
Y0
Z0

Table 5.

Full robotic mobility parameters

For the MATLAB simulation, the parameters of the robotic arm are given in table 5. The flowchart for the algorithm employed to calculate the torque history for each actuator based on the derived equations of motion (Equation 20) is shown in the Figure 18. The simulated results for the actuators torques change depending on the method of trajectory planning used to calculate the torque. For example the simulated results for the actuators torques calculated by the Third-Order Polynomial trajectory planning are presented in section 3. It can be seen that the torque history over the selected period of time (5, 10, 20 and 60 s) has considerable fluctuations.

Figure 18.

The flowchart of the dynamic model

5. The dynamic response

5.1. Introduction to the dynamic response and dynamic response analysis

The results presented in this chapter are those of chapters IV and V that is the trajectory planning and dynamic modeling respectively as they are very much related to each other. The results are divided into two sections. The first one is for the trajectory planning of the surgical robot which is divided into four parts, each represents a method from four chosen different methods for trajectory planning. As mentioned in chapter IV the joints limits for some of the joints are similar as the joints (1, 4, and 6) and (2, 5). Only the trajectory planning of similar joints will be represented. It should be noted that the results of trajectory planning for all methods were derived using four different times (5, 10, 20 and 60 s) in order to select the best time which gives the best and smooth orientation, velocity, acceleration (i.e. the inertia of the robot link) and torque. The dynamic modeling results are dependent on the trajectory planning results. This requires the results of the dynamic modeling to be represented after the trajectory planning results i.e. after comparison of the results of the trajectory planning and selecting the best time to be used.

Finally, a comparison of the results is held to choose the best method that gives the smooth set trajectory planning and best performance of the robot under investigation.The simulation results were obtained using MATLAB.

5.2. The trajectory planning analysis

As previously stated that trajectory planning was derived using four methods of the trajectory planning to select the best method that gives the smooth set trajectory planning and best performance of the robot under investigation.

The trajectory planning results is presented in two ways. The first one shows the trajectory planning of the first joint of robot derived using four different times and the second one shows the torque history for the first joint and a comparison between the four times of the first joint of robot in terms of the orientation, velocity, acceleration and the torque history to select the best time that can be used to derive the trajectory planning and torque history for all joints of robot model.

5.2.1. Third order Polynomial trajectory planning

Figures 19 and 20 show comparisons between the orientation, velocity, acceleration, torque and four different time ranges (5, 10, 20 and 60 s) of the first joint of the surgical robot. It is clearly shown in Figure 19 that the orientation behaviour increases gradualy with the time from the initial angle to the final angle and as the time increases the velocity required decreases and also the acceleration i.e. the inertia of the robot link decreases. It is also clearly shown in Figure 20 that the original torque history has considerable fluctuations. It is clear that the highest hub torque is for joint one while actuator torque of joint 6 is the lowest.

From Figures 19 and 20 the optimum final time required to complete the process for the robot can be selected. By inspection of Figure 19 for the trajectory planning of the robot we find that for all times the same behaviours i.e. the velocity and acceleration are inversely proportional with the times and the orientation behaviour increases gradualy with the time from the initial angle to the final angle. The largest time i.e. 60s to complete the results of the surgical robot can be selected. Since this time has the lowest acceleration i.e the lowest inertia. In Figure 20 for the torque history in the time 60s we find that the torque decreases with time and the over shooting decreases gradualy near the steady state time 60s.

It is seen from Table 4 that the joints angle ranges are very large. So, it is assured that the robot accelerations will not exceed the maximum permissible limits for robot’s capabilities if the robot is satisfying the trajectory in one segment.

Figure 19.

The time comparison of third order trajectory planning of the first joint

Figure 20.

The comparison between the time and torque of the first joint using third order trajectory planning

For Third-order Polynomial trajectory planning the maximum accelerations for robot’s capabilities is given by:

θ¨max=|6(θfθi)(tfti)|E21

From Figure 19 the robot acceleration needed at the beginning of the motion is 0.62 (degree/s2) as well as -0.62 (degree/s2) deceleration at the end of the motion.

Figure 21.

The orientation, angular velocity and angular acceleration for six joint micro-robot using Third order Polynomial trajectory planning

But from the equation 6.1 the maximum permissible accelerations for this robot joints is 0.6 (degree/s2). So to be ensured that robot joint accelerations will not exceed the maximum accelerations for robot’s capabilities the robot should satisfy the trajectory planning in two or more segments.

Figures 21a, b and c show the modified trajectories for the six joints, after dividing the trajectory in to two segments.

The joints angle ranges of the initial and final angles are large compared to the values of the velocity and accelerations. So it is more suitable to represent the angle-Time relation and velocity and acceleration – Time relations in two separate figures.

5.2.2. Fifth order Polynomial trajectory planning

Figures 22 and 23 show comparisons between the orientation, velocity, acceleration, and torque for four different time ranges (5, 10, 20 and 60 s) of the first joint of the robot. As is clearly shown in Figure 22 the orientation behaviour increases gradually with time from the initial angle to the final angle for the times (5, 10 and 20 s) only. But for the time 60s the orientation increases gradually from the initial angle to 40 degree then decreases to -40 degree and then increases reaching the final angle. And if the time increases the velocity required decreases. This is similar as to what is shown in Figure 23 which presents the original torque history that has considerable fluctuations. It is clear that the highest hub torque is for joint one while actuator torque of joint 6 is the lowest.

Figure 22.

The time comparison of fifth order trajectory planning of the first joint

Figure 23.

The comparison between the time and torque of the first joint using fifth order trajectory planning

From Figures 22 and 23 the optimum final time required to complete the process for the robot can be selected. It is seen from figures that the trajectory planning of the robot for the times (5, 10 and 20 s) has the same properties i.e. the velocity is inversely proportional with time. The orientation behaviour increased gradualy with the time from the initial angle to the final angle. Where the time 60s is omitted from selected. For figure 23 for the torque history in the time (5, 10, 20 and 60 s) it is found for tf=5s, the dominant part in the torque history is the inertia matrix. Increasing the final time to 10, 20 and 60s shifts the dominant term from inertia matrix to Centrifugal and Coriolis matrices. This is due to the vanishing of the acceleration at most of the joint trajectory. Another consequence of increasing the final time is the dramatic increase in the peak value of the joint torque which requires big actuator size for the same task (i.e. the same joint parameters). The time 5s to complete the results of the surgical robot can be selected. Since this time has orientation behaviour was increased gradualy with the time from the initial angle to the final angle and the torque history curves were affected by the inertia of the link of robot.

Figures 24a, b and c show the trajectories for the six joints of the robot using Fifth order Polynomial trajectory planning.

5.2.3. Linear segments with parabolic blends

Figures 25 and 26 show comparisons between the orientation, velocity, acceleration, torque and four different time ranges (5, 10, 20 and 60 s) of the first joint of the surgical robot. It is clearly shown in the Figure 25 that the orientation behaviour increases gradually with the time from the initial angle to the final angle and as the time increases the velocity required decreases and also the acceleration i.e. the inertia of the robot link decreases. It is also clearly shown in the Figure 26 that the original torque history has considerable fluctuations. It is clear that the highest hub torque is for joint one while actuator torque of joint 6 is the lowest.

Figure 24.

The orientation, angular velocity and angular acceleration for six joint micro-robot using Fifth order Polynomial trajectory planning

Figure 25.

The time comparison of liner segments with parabolic blends trajectory of the first joint

Figure 26.

The comparison between the time and torque of the first joint using liner segments with parabolic blends trajectory planning

From Figures 25 and 26 the optimum final time required to complete the process for the robot can be selected. By inspection of Figure 25 the trajectory planning has three segments they are First parabolic blends, Straight line and Second parabolic blends. The straight line segment was very important segment because the velocity in this segment is constant and the acceleration was zero i.e. no inertia of the link of the robot in this segment of time. It is therefore better to have a larger period for this segment. The largest time i.e. 60s to complete the results of the robot can be selected. Since this time has the largest period of straight line segment. By inspection of Figure 26 for the torque history in the time 60s we find that the torque decreases with the time and the shooting decreases gradualy near the steady state time 60s.

Figures 27a, b and c show the trajectories for the six joints of the robot using linear segments with parabolic blends.

Figure 27.

The orientation, angular velocity and angular acceleration for six joint micro-robot using parabolic blends trajectory planning

5.2.4. Soft motion trajectory planning

Figures 28 and 29 show comparisons between the orientation, velocity, acceleration, jerk, torque and four different time ranges (5, 10, 20 and 60 s) of the first joint of the surgical robot. It is clearly shown in the Figure 28 that the orientation behaviour increases gradually with the time from the initial angle to the final angle and as the time increases the velocity required decreases and also the acceleration i.e. the inertia of the robot link decreases. It is also clearly shown in Figure 29 that the original torque history has considerable fluctuations. It is clear that the highest hub torque is for joint one while actuator torque of joint 6 is the lowest.

Figure 28.

The time comparison of Soft motion trajectory planning of the first joint

Figure 29.

The comparison between the time and torque of the first joint using soft motion trajectory planning

Figure 30.

The orientation, angular velocity and angular acceleration for six joint micro-robot using soft motion trajectory planning

From Figures 28 and 29 we can select the optimum final time required to complete the process for the robot. By inspection of Figure 28 we find the trajectory planning three segments they are maximum jerk, maximum acceleration and maximum velocity. The maximum acceleration and maximum velocity segments were very important segments because the velocity in the maximum velocity segment is constant and the acceleration was zero i.e. no inertia of the link of the robot in this segment of time and the acceleration in the maximum acceleration segment is constant and the jerk of the link of the robot was zero. It is therefore better to have a larger period for this segment. It is seen from figures that the segments were released from time 60s but the the maximum velocity segment only was released from time 10s. Where the times (10 and 60 s) is omitted from selected i.e. the times (10 and 60 s) have properties were not satisfactory for trajectory of robot. And by inspection of Figure 29 for the torque history in the times (5 and 20 s) we find for tf=5s, the dominant part in the torque history is the inertia matrix. Increasing the final time to 20s, shift the dominant term from inertia matrix to Centrifugal and Coriolis matrices since the effect of angular velocity will be obviously high. This is due to the vanishing of the acceleration at most of the joint trajectory. Another consequence is of increasing the final time is the dramatic change in the peak value of the joint torque which requires big actuator size for the same task. So we can select the time 5s to complete the results of the robot because the torque history curve was affected by the inertia and it has the important segment i.e. maximum acceleration and maximum velocity segments.

Figures 30a, b and c show the trajectories for the six joints of the robot using soft motion trajectory planning.

5.3. The dynamic response analysis

As previously stated that the dynamic analysis of the surgical robot was derived using Lagrange-Euler technique and the results of the dynamic analysis were depended on the method of trajectory planning. In the trajectory planning results were derived using four different methods. The dynamic analysis results of surgical robot are divided into four parts each part in for each special method of the four different methods which have been derived from the trajectory planning.

Figures 31 to 34 show the original torque history which clearly shown considerable fluctuations. It is clear that the highest hub torque is experienced at joint one while actuator torque of joint 6 is the lowest.

5.3.1. The dynamic analysis results using Third order Polynomial trajectory planning

Figure 31.

Torque history for Third order Polynomial trajectory planning

5.3.2. The dynamic analysis results using Fifth order Polynomial trajectory planning

Figure 32.

Torque history for Fifth order Polynomial trajectory planning

5.3.3. The dynamic analysis results using Linear segments with parabolic blends trajectory planning

Figure 33.

Torque history for Linear segments with parabolic blends trajectory planning

5.3.4. The dynamic analysis results using Soft motion trajectory planning

Figure 34.

Torque history for Soft motion trajectory planning

6. Conclusions

A kinematic and dynamic analysis for a six-degree-of-freedom surgical robot were presented in this work. The kinematic model is based on Denavit-Hartenberg representation and the workspace of the end-effector is defined by solving the inverse kinematics problem. Four different methods were used to derive the trajectory planning for the six joints and were designed and employed to calculate the torque history for the six actuators. The dynamic equations of motion in symbolic form were derived using the Lagrange-Euler technique and the torque history was obtained using MATLAB for each joint. The proposed algorithm is flexible and can be extended to any robot configuration provided that the Denavit-Hartenberg presentation was available and the physical limits of joints are defined. The original torque history has considerable fluctuations. It was shown that the highest hub torque was of joint 1 while actuator torque of joint 6 was the lowest. It should be also noted that changing the final time for the joint trajectory changes the torque history considerably. The final time required to complete the process was selected depending on the method used to derive the trajectory planning as previously stated.

It was clearly shown in this work that the best method of trajectory planning that gives the smooth set trajectory planning and best performance of the robot under investigation was the soft motion trajectory planning because the most important reason for this selection was the torque history that has the lowest number ever of shooting and the shooting was distributed regularly over the period of time unlike the other methods which have a long number of shootings and were distributed randomly. Also the reason for selecting this method was the disappearance of the shooting quite before the final time of trajectory i.e. the steady state time. T

Appendix

Appendix (A): Notation

Cjcos θj
Sjsin θj
jthe total number of joints
ithe total number of coordinates
nthe total number of links
k and pcoefficient (1, 2, 3… n)
x, y, zlocal joint coordinates
X,Y,Zglobal joint coordinates
Yithe Kinematics equations of the end effectors
Xjthe joint variables (θ1, θ2, θ3, θ4, θ5, θ6)
Dthe end effector differential translation matrix
Dθthe joint differential motion matrix
Jthe robot Jacobian matrix (i x j)
θithe initial values of each joint angle
θfthe final values of each joint angle
tfthe time duration
θ(t)θ˙(t),θ¨(t),J(t)the orientation, angular velocity, angular acceleration and angular jerk respectively
θ¨0,θ˙0,θ0the initial conditions
TjpaJerk positive initial time
TacaAcceleration constant initial time
TjnaJerk negative initial time
TvcVelocity constant time
TjnbJerk negative final time
TacbAcceleration constant final time
TjpbJerk positive final time
qthe vector of generalized joint coordinates
q˙the vector of joint velocities
q¨the vector of joint accelerations
Mthe inertia matrix
Cthe Coriolis and centrifugal matrix
Gthe gravity matrix
Qthe vector of generalized force associated
gThe gravity matrix (1x4)
gx,gyandgzthe gravity components in x, y and z direction respectively
rj¯the location of the center of mass of link relative to the frame representing the link
Tjthe torque on joint (j)
Jithe inertia matrix
IXX,IYYandIZZthe principal moments of inertia for the link
IXY,IXZandIYZthe parallel moments of inertia for the link
mithe mass of the link
Xi¯,Yi¯andZi¯the distance between the X, Y and Z axis to the center of the link mass respectively

Appendix (B): MATLAB Code

%*******************************************************************************************************
% Micro-Robot Management
% Complete MATLAB Code for kinematic
%*******************************************************************************************************
syms th1 th2 th3 th4 th5 th6 L1 L2 L3 L4
%****************************THE INPUTS OF ROBOT *********************************************
L1=input('please enter The length of Link NO(1) (mm)=');
L2=input('please enter The length of Link NO(2) (mm)=');
L3=input('please enter The length of Link NO(3) (mm)=');
L4=input('please enter The length of Link NO(4) (mm)=');
th1min=input('please enter minth1 (degree)=');
th1max=input('please enter maxth1 (degree)=');
th2min=input('please enter minth2 (degree)=');
th2max=input('please enter maxth2 (degree)=');
th3min=input('please enter minth3 (degree)=');
th3max=input('please enter maxth3 (degree)=');
th4min=input('please enter minth4 (degree)=');
th4max=input('please enter maxth4 (degree)=');
th5min=input('please enter minth5 (degree)=');
th5max=input('please enter maxth5 (degree)=');
th6min=input('please enter minth6 (degree)=');
th6max=input('please enter maxth6 (degree)=');
%*******************************************************************************************************
th1=th1min:2:th1max;th2=th2min:th2max;th3=th3min:th3max;
th4=th4min:2:th4max;th5=th5min:th5max;th6=th6min:2:th6max;
%*******************************************************************************************************
c1=cos(th1);c2=cos(th2);c3=cos(th3);c4=cos(th4);c5=cos(th5);c6=cos(th6);
s1=sin(th1);s2=sin(th2);s3=sin(th3);s4=sin(th4);s5=sin(th5);s6=sin(th6);
%*************************D.H matrix*****************************************************************
A1=[c1,-s1,0,L1*c1;s1,c1,0,L1*s1;0,0,1,0;0,0,0,1];A2=[c2,0,s2,L2*c2;s2,0,-c2,L2*s2;0,1,0,0;0,0,0,1];
A3=[c3,0,-s3,0;s3,0,c3,0;0,-1,0,0;0,0,0,1];A4=[c4,0,-s4,0;s4,0,c4,0;0,-1,0,L3;0,0,0,1];
A5=[c5,0,-s5,0;s5,0,c5,0;0,-1,0,0;0,0,0,1];A6=[c6,-s6,0,0;s6,c6,0,0;0,0,1,L4;0,0,0,1];
A01=A1;A02=A1*A2;A03=A1*A2*A3;A04=A1*A2*A3*A4;A05=A1*A2*A3*A4*A5;
A06=A1*A2*A3*A4*A5*A6;
%*************The Kinematics equations of the end effectors**************************************
px= A06(4,1),py= A06(4,2),pz=A06(4,3)
subplot(1,3,1);plot(xa,ya);
xlabel('Px');ylabel('Py');grid on
subplot(1,3,2);plot(xa,z);
xlabel('Px');ylabel('Pz');grid on
subplot(1,3,3);plot(ya,z);
xlabel('Py');ylabel('Pz');grid on
%*********************************************THE end***********************************************
%**********************CREATED BY DR/WAEL A. AL-TABEY***********************************

© 2012 The Author(s). Licensee IntechOpen. This chapter is distributed under the terms of the Creative Commons Attribution 3.0 License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

How to cite and reference

Link to this chapter Copy to clipboard

Cite this chapter Copy to clipboard

Wael A. Al-Tabey (September 26th 2012). Micro-Robot Management, MATLAB - A Fundamental Tool for Scientific Computing and Engineering Applications - Volume 3, Vasilios N. Katsikis, IntechOpen, DOI: 10.5772/39240. Available from:

chapter statistics

3530total chapter downloads

More statistics for editors and authors

Login to your personal dashboard for more detailed statistics on your publications.

Access personal reporting

We are IntechOpen, the world's leading publisher of Open Access books. Built by scientists, for scientists. Our readership spans scientists, professors, researchers, librarians, and students, as well as business professionals. We share our knowledge and peer-reveiwed research papers with libraries, scientific and engineering societies, and also work with corporate R&D departments and government entities.

More About Us