# A New Methodology for Solving Trajectory Planning and Dynamic Load-Carrying Capacity of a Robot Manipulator

Authors: Wanjin Guo; Ruifeng Li; Chuqing Cao; Xunwei Tong; Yunfeng Gao
Journal: Mathematical Problems in Engineering
 (2016)

---

## Abstract

A new methodology using a direct method for obtaining the best found trajectory planning and maximum dynamic load-carrying capacity (DLCC) is presented for a 5-degree of freedom (DOF) hybrid robot manipulator. A nonlinear constrained multiobjective optimization problem is formulated with four objective functions, namely, travel time, total energy involved in the motion, joint jerks, and joint acceleration. The vector of decision variables is defined by the sequence of the time-interval lengths associated with each two consecutive via-points on the desired trajectory of the 5-DOF robot generalized coordinates. Then this vector of decision variables is computed in order to minimize the cost function (which is the weighted sum of these four objective functions) subject to constraints on joint positions, velocities, acceleration, jerks, forces/torques, and payload mass. Two separate approaches are proposed to deal with the trajectory planning problem and the maximum DLCC calculation for the 5-DOF robot manipulator using an evolutionary optimization technique. The adopted evolutionary algorithm is the elitist nondominated sorting genetic algorithm (NSGA-II). A numerical application is performed for obtaining best found solutions of trajectory planning and maximum DLCC calculation for the 5-DOF hybrid robot manipulator.


---

## Body

## 1. Introduction

The most primary consideration in designing and accomplishing a fundamental required task of a robot manipulator is to plan the trajectory for this robot manipulator moving from its current placement to a series of specified placements while carrying a payload. During these operations of the robot manipulator, the problem of finding such a path is referred to as the trajectory planning problem. Usually, solving an optimization problem is defined to deal with the trajectory planning problem. There are a number of popular methods for transcribing the trajectory planning problem into an optimization problem. Essentially, these transcriptions fall into two categories: direct (global) methods (such as [1–4]) and indirect (decoupled) methods (e.g., [5–8]).Solving trajectory planning for the robot manipulator is difficult because of the highly coupled nonlinear robot manipulator dynamics and complex system constraints (which include limitations of actuator forces/torques and payload constraint). A feasible and efficient method must be able to find an appropriate trajectory such that it minimizes a multicriterion cost function and satisfies the limitations. This multicriterion cost function can include travel time, energy, actuator torques, and joint jerks. In order to achieve the greatly improved productivity in industrial situations, it is necessary to minimize the total traveling time of the robot manipulator. Many research works have concentrated on minimum time trajectories [9–13]. The robot manipulator trajectory planning using the energy criteria is reported in several literatures [14–18]. This kind of manipulator trajectory planning can produce smooth trajectories and reduce the stresses between the actuators and the robot manipulator structure. Moreover, it can save energy, which may be desirable in several applications, such as aerial, space, or submarine robots with a capacity limitation of energy source. Both optimal traveling time and the minimum energy of the actuators are taken together as objective functions in some literatures [19–22]. Several examples of minimizing joint jerks and acceleration in the objective function are studied in [23–27]. Minimization of the joint jerks and acceleration usually contributes to the reduction of errors during trajectory tracking and stresses between the actuators and structure of the robot manipulator. Also, excitation frequencies of the robot manipulator can be limited.Many attempts to determine the optimal trajectory of robot manipulators are mainly conventional optimization techniques such as sequential unconstrained minimization technique, sequential quadratic programming, dynamic programming, and numerical iterative procedure. However, these conventional techniques have some drawbacks, such as easily falling into local minima or involving calculating the gradient and the Hessian of the objective function and constraints. Usually, traditional techniques cannot solve discontinuous terms (e.g., friction) in physical models. In order to overcome these drawbacks, the evolutionary methods can be applied to handle the optimization trajectory problems [23, 28]. There are several advantages of using evolutionary techniques, which include (a) population-based approach search, thus discovering the global optimal solution is possible, (b) complex and multimodal problems which can be solved for global optimality, (c) no need to provide any auxiliary relationships (like gradients, derivatives, etc.), and (d) problem independence; that is, they are suitable for solving all types of optimization problems.The dynamic load-carrying capacity (DLCC) is another important term, which is an important characteristic of the robot manipulators. It is mandatory to consider the load-carrying capacity for a robot manipulator and the payload constraint for the trajectory planning. DLCC has received considerable attention in research literature. Wang and Ravani [29, 30] determined the DLCC of a given trajectory for a serial manipulator as an optimization problem according to the state space of the dynamics equation and the torque-speed features of the actuators. Korayem et al. proposed several methods in order to find DLCC of a wheeled mobile manipulator, such as a method which is a combination of open and closed loop optimal control [31], a method based on hierarchical optimal control [32], and a method using open-loop optimal control approach and considering stability on zero-moment point criterion [33].Several classical optimization approaches have been used to deal with the trajectory planning and the DLCC calculation. Dynamic programming approach is applied to solve optimization problem with nonlinear constraints and the sequential quadratic programming formulation is used to increase the speed of convergence for both fixed and mobile base mechanical manipulators [34]. The iterative linear programming method and the discrete augmented Lagrangian technique are used to solve the nonlinear constrained optimization problem for cable driven robots [35] and parallel kinematic machines [21], respectively.In this paper, a nonlinear constrained multiobjective optimization problem is formulated with four objective functions (namely, travel time, total energy involved in the motion, joint jerks, and joint acceleration), considering constraints of joint positions, velocities, acceleration, jerks, forces/torques, and payload. The sequence of the time-interval lengths associated with each two consecutive via-points is defined as the vector of decision variables. Then the cost function (which is the weighted sum of these four objective functions) is minimized by optimizing this vector of decision variables. Using a direct method, two approaches are proposed to deal with the trajectory planning problem and the calculation of maximum dynamic load-carrying capacity for a 5-DOF robotic manipulator, respectively. An evolutionary optimization technique, that is, an elitist nondominated sorting genetic algorithm (NSGA-II), is adopted to handle these optimization problems, taking into account these objective functions and constraints. The NSGA-II algorithm was originally proposed by Deb et al. [36]. It has been proved to be a highly effective and generally successful choice for optimization problems and becomes one of the mainstream solutions to multiobjective optimization questions [37–39].The primary advantage of the proposed approaches in this paper is that the vector of decision variables (i.e., the sequence of the time-interval lengths associated with each two consecutive via-points) is optimized step by step during the evolution process (where the unfit individuals are replaced by the fit individuals through the crossover and mutation operator in each generation). The best found solutions of the joint trajectory displacement, velocity, acceleration, jerk, and force/torque can be obtained by these time-interval lengths. Additionally, the time intervals with variable lengths are more advantageous than the ones with same lengths (which are adopted by some classical optimal control approaches) because the former is more relatively likely to generate the smooth trajectory and avoid the suddenly change of the trajectory. Other advantages of the proposed approaches include the following: (a) both kinematics and dynamics aspects of the robot manipulator are considered, (b) they take several important decision criteria into account for trajectory planning (which are travel time, total energy involved in the motion, joint jerks, and joint acceleration), (c) adopting the different weights for the decision criteria, the required and suitable solutions that will satisfy various purposes can be obtained, (d) they consider payload mass constraint, (e) they ensure that the resulting best found trajectory is smooth and leads to minimum actuator power and energy, (f) they are easy to program and implement, and (g) they can also be extended to obtain the best found trajectory and the maximum DLCC for other types of robots.The rest of this paper is organized as follows. The kinematics and dynamics of the studied 5-DOF robotic manipulator are described in Section2. The problem formulation is presented in Section 3. An approach is proposed for the trajectory planning problem of the robot manipulator in Section 4. The maximum DLCC of the robot manipulator is calculated by a proposed approach in Section 5. The effectiveness of two proposed approaches for the trajectory planning and the maximum DLCC calculation is verified via a numerical example of this 5-DOF hybrid robot manipulator in Section 6. Finally, conclusions are drawn in Section 7.


## 2. Kinematic and Dynamic Modelling of the 5-DOF Robotic Manipulator

In this section, the kinematic and dynamics modelling of a 5-DOF robot manipulator are described. The structure of this robotic manipulator is shown in Figure1. This robotic manipulator, consisting of a 3-DOF (3T) parallel module and a 2-DOF (2R) serial module, is a new 3T2R hybrid robot manipulator. Translational motion axes J1, J2, and J3 (consisting of ball screws) are driven by motors M1, M2, and M3, respectively. Rotational motion axes J4 and J5 are driven by motors M4 and M5, respectively. The axis J6 is the end-effector which contains a high-speed motor spindle. More details of the structure description for this robot manipulator are described in [40].Figure 1
Robot manipulator: (a) top view; (b) front view; and (c) 3D model.
(a) (b) (c)


### 2.1. Kinematic Modelling

Based on the Denavit-Hartenberg method [41], the reference frames and the kinematic modelling for this robot manipulator are presented.


#### 2.1.1. Forward Kinematics

The reference frames for this robot manipulator are assigned as shown in Figure2(a). The vector rci (shown in Figure 2(b)) is the location of the center of mass of a link i relative to the frame representing the link. The rotation angle of J1, J2, J3, J4, and J5 is represented by φ1, φ2, φ3, φ4, and φ5, respectively. The stroke of the nut along S1, S2, and S3 is represented by XS1, XS2, and XS3, respectively. The length of the rod i is denoted by Li(i=1,2,…,6). The distance between J6 and J4 in the horizontal direction is denoted as e. The rotation angle of Jt and link 1 is denoted as α. L01 represents the distance between point O2 and point O1 in the vertical direction when the nut is located at the top starting point along S3.Figure 2
The assigned reference frame: (a) D-H reference frame and (b) the location of the center of mass.
(a) (b)The detailed structure parameters of this robot manipulator are listed in Table1. Considering the reduction ratio of the speed reducer, the pitch, the nominal torque of the AC servomotor, and the suitable safety coefficients, the constraints of displacement, velocity, acceleration, jerk, and force/torque (i.e., generalized force) are given in Table 2. Some structure parameter variables are related by(1)α=arctan⁡XS2-XS1L1,(2)XS1=sxφ12π,(3)XS2=sxφ22π,(4)XS3=szφ32π,where the constant sx represents the pitch of S1 and S2, the constant sz represents the pitch of S3, and the available range of the rotation angle is α∈[-π/4,π/4] because of the mechanical constraints.Table 1
Robot manipulator structure parameters (mm) and the lower and uppermp.
L 1 L 2 L 3 L 4 L 5 L 6 e L t L 01 s x s z m p l m p u 420 50 450 180 180 80 0 225 400 10 10 0 10Table 2
Limiting parameters.
Joint Q C min ⁡ Q C max ⁡ V C W C J C U C 1 0 mm 1000 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 2 0 mm 1000 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 3 0 mm 500 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 4 - π rad π rad 1.96 rad/s 60 rad/s2 1200 rad/s3 17 N⋅m 5 - π rad π rad 1.96 rad/s 60 rad/s2 1200 rad/s3 17 N⋅mLink 1 is driven in parallel byM1 and M2, as shown in Figures 1 and 2, and a coupled movement is generated. While link 1 is rotating about Z1 and Jt, its moving direction along the axis X1 is always parallel to X0. The base reference frame 0 is defined to be the inertial reference frame. Based on this movement relationship, the homogeneous transformation matrix 0T1 which refer to the inertial reference frame is directly given as(5)
T
0
1
=
cos
⁡
α
-
sin
⁡
α
0
X
S
1
+
X
S
2
2
sin
⁡
α
cos
⁡
α
0
0
0
0
1
0
0
0
0
1
,where(6)cos⁡α=L1XS2-XS12+L12,(7)sin⁡α=XS2-XS1XS2-XS12+L12.LetLP be defined as the tool compensation, which is the value of the tool frame point O5 in Z4 axis given as(8)Lp=Lt-L4+2L6+e.Based on the D-H reference frame, using the attached reference frames to each link, the four parameters that locate one frame relative to another are defined as follows:ai is the distance from Zi-1 to Zi along Xi-1, αi is the angle from Zi-1 to Zi about Xi-1, di is the distance from Xi-1 to Xi along Zi, and θi is the angle from Xi-1 to Xi about Zi. These geometric parameters of this manipulator for the links 2, 3, 4, and 5 (tool) shown in Figures 1 and 2 are listed in Table 3.Table 3
Geometric parameters of links.
Linki θ i α i d i a i 2 0 0 X S 3 + L 01 L 3 3 - π / 2 + φ 4 π / 4 L 2 + L 4 + 2 L 5 0 4 π + φ 5 π / 4 2 e 0 5 (tool) 0 0 L p 0One vector is represented byVi-1 in the frame i-1 and Vi in the frame i, respectively; the relationship between Vi-1 and Vi can be expressed by Vi-1=Ti-1iVi. The homogeneous transformation matrix Ti-1i is conducted to provide a representation of reference frame i relative to reference frame i-1 according to the D-H reference frames and the geometric parameters. It is given as(9)Ti-1i=cos⁡θi-cos⁡αisin⁡θisin⁡αisin⁡θiaicos⁡θisin⁡θicos⁡αicos⁡θi-sin⁡αicos⁡θiaisin⁡θi0sin⁡αicosαidi0001,where i=2,3,4,5.By successive multiplications of the transformation matrices, the equivalent homogeneous transformation matrix0T5 can be obtained. Let a vector be represented as r5 in the end-effector reference frame; then this vector related to the inertial reference frame can be expressed as r0=T05r5. Here, r0 relates the inertial reference frame. The matrix 0T5 is obtained as(10)0T5=T01T12T23T34T45=nxoxaxpxnyoyaypynzozazpz0001=Tq,where the vector p=[px,py,pz]T is the position vector of the origin point O5 relative to the inertial reference frame. The vectors a=[ax,ay,az]T, o=[ox,oy,oz]T and n=[nx,ny,nz]T are the approach vector, orientation vector, and normal vector of the end-effector reference frame 5 relative to the inertial reference frame, respectively. The rotation matrix of the end-effector reference frame 5 relative to the inertial reference frame can be expressed as 0R5=noa. The details are as follows:(11)p=px,py,pzT,(12)px=XS1+XS22-ecos⁡α+φ4+L3cos⁡α+Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52,(13)py=L3sin⁡α-esin⁡α+φ4+Lpsin⁡α+φ4cos⁡φ5-12+cos⁡α+φ4sin⁡φ52,(14)pz=L2+L4+2L5+XS3+L01+e+Lp1+cos⁡φ52,(15)a=ax,ay,azT,(16)ax=cos⁡α+φ4cosφ5-12-sin⁡α+φ4sin⁡φ52,(17)ay=sin⁡α+φ4cos⁡φ5-12+cos⁡α+φ4sin⁡φ52,(18)az=1+cos⁡φ52,(19)o=ox,oy,ozT,(20)ox=sin⁡α+φ4sin⁡φ52-cos⁡α+φ41+cos⁡φ52,(21)oy=-cos⁡α+φ4sin⁡φ52-sin⁡α+φ41+cos⁡φ52,(22)oz=1-cos⁡φ52,(23)n=nx,ny,nzT,(24)nx=-sin⁡α+φ4cos⁡φ5-cos⁡α+φ4sin⁡φ52,(25)ny=cos⁡α+φ4cos⁡φ5-sin⁡α+φ4sin⁡φ52,(26)nz=-sin⁡φ52.Let the reference point of the end-effector be represented by the vectorxe,ye,zeT relative to the end-effector reference frame 5; the inertial position coordinates x,y,z of the end-effector can be obtained by(27)xyz1=0T5xeyeze1;namely,(28)x=px+nxxe+oxye+axzey=py+nyxe+oyye+ayzez=pz+nzxe+ozye+azze.The results of the inertial position coordinates x,y,z can be represented by the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T through substituting the above related equations into (28).


#### 2.1.2. Inverse Kinematics

The structure of this 5-DOF robot manipulator satisfies the Peiper criterion [42], so that closed-form solutions can be obtained through an algebraic method. The inverse kinematics and its concise expressions are given as follows:(29)φ5=arctan⁡2-2nz,2az-1,where the arctan⁡2 function is the four-quadrant arctangent function; for example, arctan⁡2Y,X=arg⁡X+jY∈-π,π,j=-1; that is, it returns the argument of the complex number with real part X and imaginary part Y.(30)φ4=arctan⁡2ayaz-1+axnz,axaz-1-aynz-arctan⁡2A1,B1,where(31)A1=py-Lpay+eayaz-1+axnzaz-12+nz2,B1=1-py-Lpay+eayaz-1+axnzaz-12+nz22,XS3=pz-L2+L4+2L5+L01+e+azLp,XS1=px+ecos⁡α+φ4-L3cos⁡α-Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52-L1tan⁡α2,XS2=px+ecos⁡α+φ4-L3cos⁡α-Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52+L1tan⁡α2.Since the available range of the rotation angle is α∈-π/4,π/4 as mentioned above, (32)α=arctan⁡2sin⁡α,1-sin2α,sin⁡α=py-Lpay+eayaz-1+axnz/az-12+nz2L3.


#### 2.1.3. Representation of Desired End-Effector Inertial Position Trajectory

Assume that the coordinates of the desired inertial position trajectory of the 5-DOF robot end-effector denoted byxd,yd,zd are specified in the following two forms (see example in Section 6).(a) Implicit Form (33) z d t = f x d t , y d t  t ∈ t 0 , t f ,where fxdt,ydt is a given smooth function and the initial and final positions xdt0,ydt0,zdt0 and xdtf,ydtf,zdtf are given.The implicit form is used in the formulation of the basic multiobjective optimization problem (see Section3.1).(b) Parametric Form (34) x d b = f 1 b , y d b = f 2 b , z d b = f 3 b , b ∈ b 0 ,  b f ,where b is a parameter, fib,i=1,2,3, are given smooth functions, and the initial and final positions xdb0,ydb0,zdb0 and xdbf,ydbf,zdbf are given.The parametric form is used in the formulation of the finite dimensional multiobjective optimization problem (see Section3.2).Given a 5-DOF robot motion trajectoryqtt∈[t0,tf], then the resulting actual inertial position of the end-effector, denoted by xt,yt,ztt∈[t0,tf], is computed by using (28).Assume that the actual inertial position of the end-effectorxt,yt,ztt∈[t0,tf] matches the desired inertial position trajectory specified in implicit form; see (33). Then, it follows that xt,yt,ztt∈[t0,tf] satisfies the following constraint equation:(35)zt=fxt,ytt∈t0,tf,xt0,yt0,zt0=xdt0,ydt0,zdt0,xtf,ytf,ztf=xdtf,ydtf,zdtf.


### 2.2. Dynamic Modelling Using the RNEA

Because of the need for computational efficiency, the robotics community has focused on this problem of the robotic manipulator dynamics. For inverse dynamics, the recursive Newton–Euler algorithm (RNEA) proposed by Luh et al. [43] remains the most important algorithm [44]. The RNEA uses a Newton–Euler formulation of the problem, and this formulation is especially amenable to the development of efficient recursive algorithms for dynamics computations.The RNEA is an algorithm with calculation complexity ofO(n) (where n is the number of degrees of freedom in the mechanism), which is aimed to calculate recursive dynamics of fixed base open-loop robot. The calculation divides into four steps shown as follows, with two steps in each of the two recursions.Step 1.
Calculate the velocity and acceleration of each link in sequence, beginning with the known velocity and acceleration of the fixed base and calculating towards the end link of robot. The velocity of each link in the robot is obtained by the recursive formula(36)vi=vpi+Φiq˙i,v0=0,where vi is the velocity of link i, Φi is the motion matrix of joint i, and q˙i is the vector of joint velocity variables for joint i.The equivalent formula for acceleration is given by differentiating (36):(37)ai=api+Φiq¨i+Φ˙iq˙i,a0=0,where ai is the acceleration of link i and q¨i is the vector of joint acceleration variables.Through initializinga0 to -ag instead of zero (where ag is the gravitational acceleration vector and the gravity is simulated by a fictitious base acceleration), the effect of a uniform gravitational field on robot can be simulated. In this case, ai is not the true acceleration of link i, but the sum of its true acceleration and -ag.Step 2.
Calculate the equation of motion for each link. This step computes the forces required to cause the acceleration calculated in Step1. The equation of motion for link i is shown as(38)fia=Iiai+vi×Iivi,where Ii is the spatial inertia of linki and fia is the net force acting on linki.Step 3.
Calculate the spatial force on each joint. According to Figure3, the force acting on link i is as follows:(39)fia=fie+fi-∑j∈cifj,where fi is the force transmitted across jointi, fie is the sum of all relevant external forces acting on link i, and li is the connected set of link i. Rearranging (38) gives the recursive formula for calculating the joint forces:(40)fi=fia-fie+∑j∈lifj,where i iterates from NB to 1.Figure 3
Forces acting on linki.Step 4.
Calculate the joint force variables,τi. They are given by the equation(41)τi=ΦiTfi.In this section, the dynamics problem (i.e., inverse dynamics problem) of this 5-DOF robot manipulator is solved based on this very efficient RNEA. The velocity and acceleration of each link and the resultant forces on each link are first computed through an outward recursion in turn, starting with the known velocity and acceleration of the fixed base and working towards the tips. A second, inward recursion uses the force balance equations at each link to compute the spatial force across each joint and the value of each joint torque/force variable. Note that, without loss of generality, only the case of the rigid robot manipulator is stressed here; other contributing factors in the dynamic description of the robot manipulator (which may include the dynamics of the actuators, joint and link flexibility, friction, noise, and disturbances) are not considered in this paper.Define thatωij, vij, ω˙ij, v˙ij, fij, and nij are the angular velocity, the linear velocity, the angular acceleration, the linear acceleration, the force, and the moment of the reference frame i with respect to the inertial reference frame and expressed in the reference frame j, respectively. Also define that v˙cij, fcij, and ncij are the linear acceleration of the center of mass of link i, the inertial force, and the inertial moment acting on the center of mass of link i in the reference frame i with respect to the inertial reference frame and expressed in the reference frame j, respectively.The vectorzij is defined as the axis Zi of the reference frame i expressed in the reference frame j. The vector Pij is defined as the origin of the reference frame i with respect to the reference frame j and expressed in the reference frame j. The rotation matrix Rij is defined as the reference frame i with respect to the reference frame j and expressed in the reference frame j.The mass (mi), center of mass (rcii), and moment of inertia about the center of mass (Iici) of link i expressed in reference frame i are listed in Table 4.Table 4
The mass, moment of inertia matrix, and center of mass and of the robot manipulator.
Link Mass (kg) Moment of inertia matrix (kg⋅(mm)2) Center of mass (mm) 1 83.025 5095170.782 - 61.008 1019.065 - 61.008 3291706.841 - 15.858 1019.065 - 15.858 2367692.766 0 0 - 44.182 2 21.333 1475990.573 - 3970.014 - 748196.633 - 3970.014 2366543.574 - 8327.223 - 748196.633 - 8327.223 911968.216 - 304.563 - 1.306 - 278.925 3 8.022 142265.977 - 0.114 0.002 - 0.114 45016.139 46002.096 0.002 46002.096 109393.601 0 - 63.686 - 468.138 4 9.120 119573.680 27.342 - 0.849 27.342 36160.108 - 18452.465 - 0.849 - 18452.465 94076.578 0 - 52.909 - 261.241


#### 2.2.1. Outward Recursion

(a) Link 1.As shown in Figures 1 and 2, link 1 is driven in parallel by M1 and M2, and a coupled movement is generated. The angular and linear velocity and acceleration of reference frame 1 representing link 1 with respect to the inertial reference frame and expressed in the inertial reference frame can be directly given as follows:(42)ω10=0,0,α˙T,v10=X˙S1+X˙S22,0,0T,ω˙10=0,0,α¨T,v˙10=X¨S1+X¨S22,0,-gT,where g=9.80665 m/s2, according to (1), yielding(43)α˙=L1X˙S2-X˙S1L12+XS2-XS12,α¨=L1X¨S2-X¨S1L12+XS2-XS12-2L1XS2-XS1X˙S2-X˙S12L12+XS2-XS122.The angular and linear velocity and acceleration of the reference frame 1 and the linear acceleration of the center of mass of link 1 and the inertial force and the inertial moment acting on the center of mass of link 1 in the reference frame 1, with respect to the inertial reference frame and expressed in the reference frame 1, are derived as(44)ω11=R01ω10,(45)v11=R01v10,(46)ω˙11=R01ω˙10,(47)v˙11=R01v˙10,(48)v˙c11=v˙11+ω˙11×rc11+ω11×ω11×rc11,(49)fc11=m1v˙c11,(50)nc11=I1c1ω˙11+ω11×I1c1ω11,where the rotation matrix R01 is given by(51)R01=cos⁡αsin⁡α0-sin⁡αcos⁡α0001.(b) Link 2. The angular and linear velocity and acceleration of the reference frame 2 and the linear acceleration of the center of mass of link 2 and the inertial force and the inertial moment acting on the center of mass of link 2 in the reference frame 2, with respect to the inertial reference frame and expressed in the reference frame 2, are derived as(52)ω22=R12ω11,(53)ω˙22=R12ω˙11,(54)v˙22=R12v˙11+ω˙11×P21+ω11×ω11×P21+2ω22×z12X˙S3+2z1X¨S3,(55)v˙c22=v˙22+ω˙22×rc22+ω22×ω22×rc22,(56)fc22=m2v˙c22,(57)nc22=I2c2ω˙22+ω22×I2c2ω22,where(58)P21=L3,0,XS3+L01T,z12=0,0,1T,R12=100010001.(c) Link 3. The angular and linear velocity and acceleration of the reference frame 3 and the linear acceleration of the center of mass of link 3 and the inertial force and the inertial moment acting on the center of mass of link 3 in the reference frame 3, with respect to the inertial reference frame and expressed in the reference frame 3, are calculated as follows:(59)ω33=R23ω22+z23φ˙4,(60)ω˙33=R23ω˙22+ω22×z23φ˙4+z23φ¨4,(61)v˙33=R23v˙22+ω˙22×P32+ω22×ω22×P32,(62)v˙c33=v˙33+ω˙33×rc33+ω33×ω33×rc33,(63)fc33=m3v˙c33,(64)nc33=I3c3ω˙33+ω33×I3c3ω33,where(65)P32=0,0,L2+L4+2L5T,z23=0,22,22T,R23=sin⁡φ4-cos⁡φ40cos⁡φ42sin⁡φ4222-cos⁡φ42-sin⁡φ4222.(d) Link 4.The angular and linear velocity and acceleration of the reference frame 4 and the linear acceleration of the center of mass of link 4 and the inertial force and the inertial moment acting on the center of mass of link 4 in the reference frame 4, with respect to the inertial reference frame and expressed in the reference frame 4, are given as follows:(66)ω44=R34ω33+z34φ˙5,(67)ω˙44=R34ω˙33+R34ω33×z34φ˙5+z34φ¨5,(68)v˙44=R34v˙33+ω˙33×P43+ω33×ω33×P43,(69)v˙c44=v˙44+ω˙44×rc44+ω44×ω44×rc44,(70)fc44=m4v˙c44,(71)nc44=I4c4ω˙44+ω44×I4c4ω44,where(72)P43=0,0,2eT,z34=0,22,22T,R34=-cos⁡φ5-sin⁡φ50sin⁡φ52-cos⁡φ5222-sin⁡φ52cos⁡φ5222.


#### 2.2.2. Inward Recursion

For each link of robot manipulator, except the gravitational force, the forces and moments acting one link belong to internal forces and moments acting between the connected links; that is, the external forces and moments relationships between connected links are forces/moments and reaction forces/moments. A diagram is given for displaying the external forces and resultant forces (shown in Figure4). External forces and moments acting on link i are shown in Figure 4(a). Here, the force and moment of link i-1 acting on link i are resultant fii and nii, respectively; also, the force and moment of link i+1 acting on link i are resultant -fi+1i and -ni+1i, respectively; mig is the gravitational force of link i (where mi is the mass of link i and g is gravitational acceleration vector). The resultant force and moment at the center of mass of link i caused by these external forces and moments acting on link i based on static balancing conditions are fcii and ncii (shown in Figure 4(b)), respectively.Figure 4
External forces and resultant forces of linki: (a) external forces and moments acting on link i and (b) resultant force and moment at the center of mass of link i.
(a) (b)(a) Link 4.When a robot performs a manipulation task (e.g., polishing a surface), the robot establishes physical contact with external environment; the relevant external forces and/or moments will act on the end-effector. In order to express this influence in the dynamic equations of motion, suppose that the resultant of all relevant external forces and moments applied on the origin of the end-effector reference frame 5 is ftool and ntool, respectively. Thus, the force and moment of link 4 applied on the origin of the reference frame 5 are f55=-ftool and n55=-ntool, respectively. The payload mass mp can be entered as an external force ftool.The force and the moment of the reference frame 4 with respect to the inertial reference frame and expressed in the reference frame 4 and the required joint actuator torque of joint 5 are derived as(73)f44=R54f55+fc44,(74)n44=R54n55+nc44+rc44×fc44+P54×R54f55,(75)τ5=4z3Tn44,where(76)P54=0,0,LpT,R54=100010001.Note that, for link 4, the inertial forcefc44 and the inertial moment nc44 (which are expressed in (70) and (71), resp.) are represented as the term nc44+rc44×fc44 in (74) in order to apply the influence of the center of mass of link 4 on the origin of the reference frame 4. Other influences of the center of mass of link i(i=1,2,3) have the same representation forms.(b) Link 3. The force and the moment of the reference frame 3 with respect to the inertial reference frame and expressed in the reference frame 3 and the required joint actuator torque of joint 4 are obtained as(77)f33=R43f44+fc33,(78)n33=4R3Tn44+nc33+rc33×fc33+P43×4R3Tf44,(79)τ4=3z2T3n3.(c) Link 2. The force and the moment of the reference frame 2 with respect to the inertial reference frame and expressed in the reference frame 2 and the required joint actuator torque of joint 3 are expressed as follows:(80)f22=R32f33+fc22,(81)n22=R2T3n33+nc22+rc22×fc22+P32×R2T3f33,(82)F3=2z1Tf22.(d) Link 1. The force and the moment of the reference frame 1 with respect to the inertial reference frame and expressed in the reference frame 1 are given as follows:(83)f11=R21f22+fc11,(84)n11=R21n22+nc11+rc11×fc11+P21×R21f22.The interaction forces between link 1 and screw joints (consisting of ball screws) are shown in Figure5. Combining Newton’s and Euler’s equations for the link 1 leads to(85)L1F2-F12cos2⁡α=1z0TR10n11F2+F1=1x0TR10f11,where z01=0,0,1T and x01=1,0,0T.Figure 5
The interaction forces: (a) screw joints and (b) link 1.
(a) (b)The required joint actuator forces of joint 1 and joint 2 are derived by (85); that is,(86)F1=x10TR10f112-z10TR10n11cos2⁡αL1,(87)F2=x10TR10f112+z10TR10n11cos2⁡αL1.Ignoring transmission power loss, the resulting five applied motor torques (while the five reaction torques are equal in magnitude and opposite in direction) can be obtained as(88)TM1=F1sx2π,TM2=F2sx2π,TM3=F3sz2π,TM4=τ4,TM5=τ5.Thus, given a 5-DOF robot trajectoryqt,q˙t,q¨tt∈[t0,tf], then the applied forces and torques U=F1F2F3τ4τ5T required to produce the above-mentioned trajectory are computed as follows.Step 1.
Calculate the velocity and acceleration of each link in turn, starting with the known velocity and acceleration of the fixed base, and working towards the end. They are computed by (44)–(47), (52)–(54), (59)–(61), and (66)–(68).Step 2.
Calculate the equation of motion for each link. They are determined by (50), (57), (64), and (71).Step 3.
Calculate the spatial force on each joint. They are given by (74), (78), (81), and (84).Step 4.
Calculate the joint force variables (i.e., the applied forces and torquesU=F1F2F3τ4τ5T). They are obtained by (75), (79), (82), (86), and (87).


### 2.3. Dynamic Modelling Using the Lagrange Equations

Besides the RNEA, the robot dynamic equations of motion can also be derived by the Lagrange equations. The dynamic equations can be deduced by calculating the kinetic and potential energies of the links and by defining the Lagrangian and by differentiating the Lagrangian equation with respect to the joint variables [45].


#### 2.3.1. Kinetic Energy

As mentioned above, the link 1 is driven in parallel byM1 and M2, and a coupled movement is generated. In order to separately express this coupled movement, an addition virtual link with zero mass and zero length is introduced between the base and link 1 to produce a translational motion. Thus, a translational motion of the virtual link along the axis X1 and a rotation motion of the link 1 about the axis Z1 can be decoupled from this coupled movement. The corresponding transformation matrix 0T1 can be represented as(89)T10=A1A2,A1=100d1010000100001,A2=cos⁡ϕ2-sin⁡ϕ200sin⁡ϕ2cos⁡ϕ20000100001,where(90)d1=XS1+XS22,ϕ2=α.Other transformation matricesi-1Ti for the link i(i=2,3,4) are represented by Aj(j=3,4,5), respectively; that is,(91)Aj=Tii-1i=2,3,4j=i+1,where the joint variables are given as follows:(92)d3=XS3+L01,ϕ4=-π2+φ4,ϕ5=π+φ5.The transformation between the reference framei and the inertial reference frame can be written as(93)Ti0=T10T21⋯Tii-1=A1A2⋯Aii=1,2,…,5.Using local generalized coordinatesψ=d1,ϕ2,d3,ϕ4,ϕ5T to represent these joint variables (d1 and d3 for prismatic joints and ϕ1, ϕ4 and ϕ5 for revolute joints), the derivative of Ai matrix for a revolute joint or a prismatic joint can be expressed as(94)∂Ai∂ψi=ρiAii=1,2,…,5,where(95)ρirevolute=0-100100000000000,ρiprismatic=0000000000010000.Extending the same differentiation principle to the0Ti matrix of (93) with multiple joint variables differentiated with respect to only one variable qj gives(96)Γij=∂Ti0∂ψj=∂A1A2⋯Aj⋯Ai∂ψj=A1A2⋯QjAj⋯Aij≤i≤5.Higher-order derivatives can be formulated similarly from(97)Γijk=∂Γij∂ψk.Usingri to represent a point on any link i of the robot relative to frame i, the position of the point can expressed by premultiplying the vector with the transformation matrix representing its frame as follows:(98)pi=Ti0ri.Differentiating (98) with respect to all the local variables ψj yields the velocity of the point:(99)Vi=ddtTi0ri=∑j=1i∂Ti0∂ψjdψjdtri=∑j=1iΓijdψjdtri.The kinetic energy of an element of massmi on a link is(100)dKi=12Trace∑p=1iΓipdψpdtri∑r=1iΓirdψrdtriTdmi,where the mathematical symbol “Trace” is the trace of a matrix. Integrating this equation and rearranging terms yield the total kinetic energy:(101)Ki=∫dKi=12Trace∑p=1i∑r=1iΓip∫ririTdmiΓirTψ˙pψ˙r.The Pseudo Inertia Matrix, representing the∫ririTdmi terms, can be written as(102)Ji=-Ixx+Iyy+Izzi2IixyIixzmixlciIixyIxx-Iyy+Izzi2IiyzmiylciIixzIiyzIxx+Iyy-Izzi2mizlcimixlcimiylcimizlcimi,where rlci=xlci,ylci,zlciT is the location of the center of mass of the link i relative to the frame representing the link. For the local generalized variables ψ=d1,ϕ2,d3,ϕ4,ϕ5T, the corresponding Pseudo Inertia Matrices are J1, J2, J3, J4, and J5, respectively. The Pseudo Inertia Matrices and the location of the center of mass of the studied robot manipulator are listed in Table 5.Table 5
The mass (mi), Pseudo Inertia Matrix (Ji), and the location of the center of mass (rlci).
Number m i J i r l c i 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 2 83.025 282114.412 - 61.008 1019.065 0 - 61.008 2085578.353 - 15.858 0 1019.065 - 15.858 3009592.428 - 3668.210 0 0 - 3668.210 83.025 0 0 - 44.182 1 3 21.333 901260.608 - 3970.014 - 748196.633 - 6497.242 - 3970.014 2085578.353 - 8327.223 - 27.860 - 748196.633 - 8327.223 3009592.428 - 3668.210 - 6497.242 - 27.860 - 3668.210 21.333 - 304.563 - 1.306 - 278.925 1 4 8.022 6071.881 - 0.114 0.002 0 - 0.114 103321.719 46002.096 - 510.889 0.002 46002.096 38944.257 - 3755.403 0 - 510.889 - 3755.403 8.022 0 - 63.686 - 468.138 1 5 9.120 5331.503 27.342 - 0.849 0.501 27.342 88745.074 - 18452.465 - 482.530 - 0.849 - 18452.465 30828.605 - 2382.517 0.501 - 482.530 - 2382.517 9.120 0 - 52.909 - 261.241 1Substituting (102) into (101) gives the final form for kinetic energy of the studied robot manipulator (here, the kinetic energy of the actuators is not stressed in this paper):(103)K=12∑i=15∑p=1i∑r=1iTraceΓipJiΓirTψ˙pψ˙r.


#### 2.3.2. Potential Energy

The potential energy of the system is the sum of the potential energies of each link and can be written as(104)P=∑i=15Pi=∑i=15-migT0Tirlci,where gT=[gxgygz0] is the gravity matrix. Notice that the values in the gravity matrix are dependent on the orientation of the reference frame.


#### 2.3.3. The Lagrangian

The Lagrangian of the robot manipulator is then expressed as(105)L=K-P=12∑i=15∑p=1i∑r=1iTraceΓipJiΓirTψ˙pψ˙r-∑i=15-migT0Tirlci.


#### 2.3.4. The Lagrangian Dynamics for the Robot’s Equations of Motion

The Lagrangian of the robot manipulator can be differentiated for local generalized coordinatesψ=d1,ϕ2,d3,ϕ4,ϕ5T in order to form the dynamic equations of motion:(106)T=∂∂t∂L∂ψ˙-∂L∂ψ.The final equations of motion for all local generalized variablesψ=d1,ϕ2,d3,ϕ4,ϕ5T can be derived as follows:(107)Ti=∑j=15Dijψ¨j+∑j=15∑k=15Dijkψ˙jψ˙k+Dii=1,2,…,5,where(108)Dij=∑p=max⁡i,j5TraceΓpjJpΓpiT,Dijk=∑p=max⁡i,j,k5TraceΓpjkJpΓpiT,Di=∑p=i5-mpgTΓpirlcp.The following derivations of transformation relationship, between equations of motionU=F1,F2,F3,τ4,τ5T (represented by the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T) and equations of motion U=F1,F2,F3,τ4,τ5T (represented by the local generalized variables ψ=d1,ϕ2,d3,ϕ4,ϕ5T), can be yielded as follows.The relationship between forcesF1 and F2, respectively, produced by the generalized coordinates XS1 and XS2 and T1 and T2, respectively, produced by the virtual generalized coordinate d1 and ϕ2 can be derived using the principle of virtual work and (90) as(109)F1δXS1+F2δXS2=T1δd1+T2δϕ2δd1=δXS1+δXS22δϕ2=cos2⁡αδXS2-δXS1L1.Based on (6) and (109), it yields(110)F1=T12-T2L1XS2-XS12+L12,F2=T12+T2L1XS2-XS12+L12.Similarly, based on (92), it gives(111)F3=T3,τ4=T4,τ5=T5.From the above derivations in this section, the dynamic equations of motion represented by the local generalized variablesψ=d1,ϕ2,d3,ϕ4,ϕ5T and the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T for the studied 5-DOF robot manipulator are obtained, respectively.Silver compared the computational efficiency between Newton–Euler and Lagrangian formulations for manipulator dynamics and proved that they are indeed equivalent to each other [46]. There is in fact no fundamental difference in computational efficiency between two. The former is significantly more efficient than the latter due to two factors: the recursive structure of the computation and the representation chosen for the rotational dynamics. Namely, the former works directly with Newton’s and Euler’s equations for a rigid body, which are contained within the spatial equation of motion. This formulation is especially amenable to the development of efficient recursive algorithms for dynamics computations. The latter only considers the total robot energy and proceeds via the Lagrangian of the robot mechanism; then the dynamic equations of motion are developed using Lagrange’s equation for each defined generalized coordinate.


## 2.1. Kinematic Modelling

Based on the Denavit-Hartenberg method [41], the reference frames and the kinematic modelling for this robot manipulator are presented.


### 2.1.1. Forward Kinematics

The reference frames for this robot manipulator are assigned as shown in Figure2(a). The vector rci (shown in Figure 2(b)) is the location of the center of mass of a link i relative to the frame representing the link. The rotation angle of J1, J2, J3, J4, and J5 is represented by φ1, φ2, φ3, φ4, and φ5, respectively. The stroke of the nut along S1, S2, and S3 is represented by XS1, XS2, and XS3, respectively. The length of the rod i is denoted by Li(i=1,2,…,6). The distance between J6 and J4 in the horizontal direction is denoted as e. The rotation angle of Jt and link 1 is denoted as α. L01 represents the distance between point O2 and point O1 in the vertical direction when the nut is located at the top starting point along S3.Figure 2
The assigned reference frame: (a) D-H reference frame and (b) the location of the center of mass.
(a) (b)The detailed structure parameters of this robot manipulator are listed in Table1. Considering the reduction ratio of the speed reducer, the pitch, the nominal torque of the AC servomotor, and the suitable safety coefficients, the constraints of displacement, velocity, acceleration, jerk, and force/torque (i.e., generalized force) are given in Table 2. Some structure parameter variables are related by(1)α=arctan⁡XS2-XS1L1,(2)XS1=sxφ12π,(3)XS2=sxφ22π,(4)XS3=szφ32π,where the constant sx represents the pitch of S1 and S2, the constant sz represents the pitch of S3, and the available range of the rotation angle is α∈[-π/4,π/4] because of the mechanical constraints.Table 1
Robot manipulator structure parameters (mm) and the lower and uppermp.
L 1 L 2 L 3 L 4 L 5 L 6 e L t L 01 s x s z m p l m p u 420 50 450 180 180 80 0 225 400 10 10 0 10Table 2
Limiting parameters.
Joint Q C min ⁡ Q C max ⁡ V C W C J C U C 1 0 mm 1000 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 2 0 mm 1000 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 3 0 mm 500 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 4 - π rad π rad 1.96 rad/s 60 rad/s2 1200 rad/s3 17 N⋅m 5 - π rad π rad 1.96 rad/s 60 rad/s2 1200 rad/s3 17 N⋅mLink 1 is driven in parallel byM1 and M2, as shown in Figures 1 and 2, and a coupled movement is generated. While link 1 is rotating about Z1 and Jt, its moving direction along the axis X1 is always parallel to X0. The base reference frame 0 is defined to be the inertial reference frame. Based on this movement relationship, the homogeneous transformation matrix 0T1 which refer to the inertial reference frame is directly given as(5)
T
0
1
=
cos
⁡
α
-
sin
⁡
α
0
X
S
1
+
X
S
2
2
sin
⁡
α
cos
⁡
α
0
0
0
0
1
0
0
0
0
1
,where(6)cos⁡α=L1XS2-XS12+L12,(7)sin⁡α=XS2-XS1XS2-XS12+L12.LetLP be defined as the tool compensation, which is the value of the tool frame point O5 in Z4 axis given as(8)Lp=Lt-L4+2L6+e.Based on the D-H reference frame, using the attached reference frames to each link, the four parameters that locate one frame relative to another are defined as follows:ai is the distance from Zi-1 to Zi along Xi-1, αi is the angle from Zi-1 to Zi about Xi-1, di is the distance from Xi-1 to Xi along Zi, and θi is the angle from Xi-1 to Xi about Zi. These geometric parameters of this manipulator for the links 2, 3, 4, and 5 (tool) shown in Figures 1 and 2 are listed in Table 3.Table 3
Geometric parameters of links.
Linki θ i α i d i a i 2 0 0 X S 3 + L 01 L 3 3 - π / 2 + φ 4 π / 4 L 2 + L 4 + 2 L 5 0 4 π + φ 5 π / 4 2 e 0 5 (tool) 0 0 L p 0One vector is represented byVi-1 in the frame i-1 and Vi in the frame i, respectively; the relationship between Vi-1 and Vi can be expressed by Vi-1=Ti-1iVi. The homogeneous transformation matrix Ti-1i is conducted to provide a representation of reference frame i relative to reference frame i-1 according to the D-H reference frames and the geometric parameters. It is given as(9)Ti-1i=cos⁡θi-cos⁡αisin⁡θisin⁡αisin⁡θiaicos⁡θisin⁡θicos⁡αicos⁡θi-sin⁡αicos⁡θiaisin⁡θi0sin⁡αicosαidi0001,where i=2,3,4,5.By successive multiplications of the transformation matrices, the equivalent homogeneous transformation matrix0T5 can be obtained. Let a vector be represented as r5 in the end-effector reference frame; then this vector related to the inertial reference frame can be expressed as r0=T05r5. Here, r0 relates the inertial reference frame. The matrix 0T5 is obtained as(10)0T5=T01T12T23T34T45=nxoxaxpxnyoyaypynzozazpz0001=Tq,where the vector p=[px,py,pz]T is the position vector of the origin point O5 relative to the inertial reference frame. The vectors a=[ax,ay,az]T, o=[ox,oy,oz]T and n=[nx,ny,nz]T are the approach vector, orientation vector, and normal vector of the end-effector reference frame 5 relative to the inertial reference frame, respectively. The rotation matrix of the end-effector reference frame 5 relative to the inertial reference frame can be expressed as 0R5=noa. The details are as follows:(11)p=px,py,pzT,(12)px=XS1+XS22-ecos⁡α+φ4+L3cos⁡α+Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52,(13)py=L3sin⁡α-esin⁡α+φ4+Lpsin⁡α+φ4cos⁡φ5-12+cos⁡α+φ4sin⁡φ52,(14)pz=L2+L4+2L5+XS3+L01+e+Lp1+cos⁡φ52,(15)a=ax,ay,azT,(16)ax=cos⁡α+φ4cosφ5-12-sin⁡α+φ4sin⁡φ52,(17)ay=sin⁡α+φ4cos⁡φ5-12+cos⁡α+φ4sin⁡φ52,(18)az=1+cos⁡φ52,(19)o=ox,oy,ozT,(20)ox=sin⁡α+φ4sin⁡φ52-cos⁡α+φ41+cos⁡φ52,(21)oy=-cos⁡α+φ4sin⁡φ52-sin⁡α+φ41+cos⁡φ52,(22)oz=1-cos⁡φ52,(23)n=nx,ny,nzT,(24)nx=-sin⁡α+φ4cos⁡φ5-cos⁡α+φ4sin⁡φ52,(25)ny=cos⁡α+φ4cos⁡φ5-sin⁡α+φ4sin⁡φ52,(26)nz=-sin⁡φ52.Let the reference point of the end-effector be represented by the vectorxe,ye,zeT relative to the end-effector reference frame 5; the inertial position coordinates x,y,z of the end-effector can be obtained by(27)xyz1=0T5xeyeze1;namely,(28)x=px+nxxe+oxye+axzey=py+nyxe+oyye+ayzez=pz+nzxe+ozye+azze.The results of the inertial position coordinates x,y,z can be represented by the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T through substituting the above related equations into (28).


### 2.1.2. Inverse Kinematics

The structure of this 5-DOF robot manipulator satisfies the Peiper criterion [42], so that closed-form solutions can be obtained through an algebraic method. The inverse kinematics and its concise expressions are given as follows:(29)φ5=arctan⁡2-2nz,2az-1,where the arctan⁡2 function is the four-quadrant arctangent function; for example, arctan⁡2Y,X=arg⁡X+jY∈-π,π,j=-1; that is, it returns the argument of the complex number with real part X and imaginary part Y.(30)φ4=arctan⁡2ayaz-1+axnz,axaz-1-aynz-arctan⁡2A1,B1,where(31)A1=py-Lpay+eayaz-1+axnzaz-12+nz2,B1=1-py-Lpay+eayaz-1+axnzaz-12+nz22,XS3=pz-L2+L4+2L5+L01+e+azLp,XS1=px+ecos⁡α+φ4-L3cos⁡α-Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52-L1tan⁡α2,XS2=px+ecos⁡α+φ4-L3cos⁡α-Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52+L1tan⁡α2.Since the available range of the rotation angle is α∈-π/4,π/4 as mentioned above, (32)α=arctan⁡2sin⁡α,1-sin2α,sin⁡α=py-Lpay+eayaz-1+axnz/az-12+nz2L3.


### 2.1.3. Representation of Desired End-Effector Inertial Position Trajectory

Assume that the coordinates of the desired inertial position trajectory of the 5-DOF robot end-effector denoted byxd,yd,zd are specified in the following two forms (see example in Section 6).(a) Implicit Form (33) z d t = f x d t , y d t  t ∈ t 0 , t f ,where fxdt,ydt is a given smooth function and the initial and final positions xdt0,ydt0,zdt0 and xdtf,ydtf,zdtf are given.The implicit form is used in the formulation of the basic multiobjective optimization problem (see Section3.1).(b) Parametric Form (34) x d b = f 1 b , y d b = f 2 b , z d b = f 3 b , b ∈ b 0 ,  b f ,where b is a parameter, fib,i=1,2,3, are given smooth functions, and the initial and final positions xdb0,ydb0,zdb0 and xdbf,ydbf,zdbf are given.The parametric form is used in the formulation of the finite dimensional multiobjective optimization problem (see Section3.2).Given a 5-DOF robot motion trajectoryqtt∈[t0,tf], then the resulting actual inertial position of the end-effector, denoted by xt,yt,ztt∈[t0,tf], is computed by using (28).Assume that the actual inertial position of the end-effectorxt,yt,ztt∈[t0,tf] matches the desired inertial position trajectory specified in implicit form; see (33). Then, it follows that xt,yt,ztt∈[t0,tf] satisfies the following constraint equation:(35)zt=fxt,ytt∈t0,tf,xt0,yt0,zt0=xdt0,ydt0,zdt0,xtf,ytf,ztf=xdtf,ydtf,zdtf.


## 2.1.1. Forward Kinematics

The reference frames for this robot manipulator are assigned as shown in Figure2(a). The vector rci (shown in Figure 2(b)) is the location of the center of mass of a link i relative to the frame representing the link. The rotation angle of J1, J2, J3, J4, and J5 is represented by φ1, φ2, φ3, φ4, and φ5, respectively. The stroke of the nut along S1, S2, and S3 is represented by XS1, XS2, and XS3, respectively. The length of the rod i is denoted by Li(i=1,2,…,6). The distance between J6 and J4 in the horizontal direction is denoted as e. The rotation angle of Jt and link 1 is denoted as α. L01 represents the distance between point O2 and point O1 in the vertical direction when the nut is located at the top starting point along S3.Figure 2
The assigned reference frame: (a) D-H reference frame and (b) the location of the center of mass.
(a) (b)The detailed structure parameters of this robot manipulator are listed in Table1. Considering the reduction ratio of the speed reducer, the pitch, the nominal torque of the AC servomotor, and the suitable safety coefficients, the constraints of displacement, velocity, acceleration, jerk, and force/torque (i.e., generalized force) are given in Table 2. Some structure parameter variables are related by(1)α=arctan⁡XS2-XS1L1,(2)XS1=sxφ12π,(3)XS2=sxφ22π,(4)XS3=szφ32π,where the constant sx represents the pitch of S1 and S2, the constant sz represents the pitch of S3, and the available range of the rotation angle is α∈[-π/4,π/4] because of the mechanical constraints.Table 1
Robot manipulator structure parameters (mm) and the lower and uppermp.
L 1 L 2 L 3 L 4 L 5 L 6 e L t L 01 s x s z m p l m p u 420 50 450 180 180 80 0 225 400 10 10 0 10Table 2
Limiting parameters.
Joint Q C min ⁡ Q C max ⁡ V C W C J C U C 1 0 mm 1000 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 2 0 mm 1000 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 3 0 mm 500 mm 250 mm/s 5000 mm/s2 100000 m/s3 500 N 4 - π rad π rad 1.96 rad/s 60 rad/s2 1200 rad/s3 17 N⋅m 5 - π rad π rad 1.96 rad/s 60 rad/s2 1200 rad/s3 17 N⋅mLink 1 is driven in parallel byM1 and M2, as shown in Figures 1 and 2, and a coupled movement is generated. While link 1 is rotating about Z1 and Jt, its moving direction along the axis X1 is always parallel to X0. The base reference frame 0 is defined to be the inertial reference frame. Based on this movement relationship, the homogeneous transformation matrix 0T1 which refer to the inertial reference frame is directly given as(5)
T
0
1
=
cos
⁡
α
-
sin
⁡
α
0
X
S
1
+
X
S
2
2
sin
⁡
α
cos
⁡
α
0
0
0
0
1
0
0
0
0
1
,where(6)cos⁡α=L1XS2-XS12+L12,(7)sin⁡α=XS2-XS1XS2-XS12+L12.LetLP be defined as the tool compensation, which is the value of the tool frame point O5 in Z4 axis given as(8)Lp=Lt-L4+2L6+e.Based on the D-H reference frame, using the attached reference frames to each link, the four parameters that locate one frame relative to another are defined as follows:ai is the distance from Zi-1 to Zi along Xi-1, αi is the angle from Zi-1 to Zi about Xi-1, di is the distance from Xi-1 to Xi along Zi, and θi is the angle from Xi-1 to Xi about Zi. These geometric parameters of this manipulator for the links 2, 3, 4, and 5 (tool) shown in Figures 1 and 2 are listed in Table 3.Table 3
Geometric parameters of links.
Linki θ i α i d i a i 2 0 0 X S 3 + L 01 L 3 3 - π / 2 + φ 4 π / 4 L 2 + L 4 + 2 L 5 0 4 π + φ 5 π / 4 2 e 0 5 (tool) 0 0 L p 0One vector is represented byVi-1 in the frame i-1 and Vi in the frame i, respectively; the relationship between Vi-1 and Vi can be expressed by Vi-1=Ti-1iVi. The homogeneous transformation matrix Ti-1i is conducted to provide a representation of reference frame i relative to reference frame i-1 according to the D-H reference frames and the geometric parameters. It is given as(9)Ti-1i=cos⁡θi-cos⁡αisin⁡θisin⁡αisin⁡θiaicos⁡θisin⁡θicos⁡αicos⁡θi-sin⁡αicos⁡θiaisin⁡θi0sin⁡αicosαidi0001,where i=2,3,4,5.By successive multiplications of the transformation matrices, the equivalent homogeneous transformation matrix0T5 can be obtained. Let a vector be represented as r5 in the end-effector reference frame; then this vector related to the inertial reference frame can be expressed as r0=T05r5. Here, r0 relates the inertial reference frame. The matrix 0T5 is obtained as(10)0T5=T01T12T23T34T45=nxoxaxpxnyoyaypynzozazpz0001=Tq,where the vector p=[px,py,pz]T is the position vector of the origin point O5 relative to the inertial reference frame. The vectors a=[ax,ay,az]T, o=[ox,oy,oz]T and n=[nx,ny,nz]T are the approach vector, orientation vector, and normal vector of the end-effector reference frame 5 relative to the inertial reference frame, respectively. The rotation matrix of the end-effector reference frame 5 relative to the inertial reference frame can be expressed as 0R5=noa. The details are as follows:(11)p=px,py,pzT,(12)px=XS1+XS22-ecos⁡α+φ4+L3cos⁡α+Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52,(13)py=L3sin⁡α-esin⁡α+φ4+Lpsin⁡α+φ4cos⁡φ5-12+cos⁡α+φ4sin⁡φ52,(14)pz=L2+L4+2L5+XS3+L01+e+Lp1+cos⁡φ52,(15)a=ax,ay,azT,(16)ax=cos⁡α+φ4cosφ5-12-sin⁡α+φ4sin⁡φ52,(17)ay=sin⁡α+φ4cos⁡φ5-12+cos⁡α+φ4sin⁡φ52,(18)az=1+cos⁡φ52,(19)o=ox,oy,ozT,(20)ox=sin⁡α+φ4sin⁡φ52-cos⁡α+φ41+cos⁡φ52,(21)oy=-cos⁡α+φ4sin⁡φ52-sin⁡α+φ41+cos⁡φ52,(22)oz=1-cos⁡φ52,(23)n=nx,ny,nzT,(24)nx=-sin⁡α+φ4cos⁡φ5-cos⁡α+φ4sin⁡φ52,(25)ny=cos⁡α+φ4cos⁡φ5-sin⁡α+φ4sin⁡φ52,(26)nz=-sin⁡φ52.Let the reference point of the end-effector be represented by the vectorxe,ye,zeT relative to the end-effector reference frame 5; the inertial position coordinates x,y,z of the end-effector can be obtained by(27)xyz1=0T5xeyeze1;namely,(28)x=px+nxxe+oxye+axzey=py+nyxe+oyye+ayzez=pz+nzxe+ozye+azze.The results of the inertial position coordinates x,y,z can be represented by the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T through substituting the above related equations into (28).


## 2.1.2. Inverse Kinematics

The structure of this 5-DOF robot manipulator satisfies the Peiper criterion [42], so that closed-form solutions can be obtained through an algebraic method. The inverse kinematics and its concise expressions are given as follows:(29)φ5=arctan⁡2-2nz,2az-1,where the arctan⁡2 function is the four-quadrant arctangent function; for example, arctan⁡2Y,X=arg⁡X+jY∈-π,π,j=-1; that is, it returns the argument of the complex number with real part X and imaginary part Y.(30)φ4=arctan⁡2ayaz-1+axnz,axaz-1-aynz-arctan⁡2A1,B1,where(31)A1=py-Lpay+eayaz-1+axnzaz-12+nz2,B1=1-py-Lpay+eayaz-1+axnzaz-12+nz22,XS3=pz-L2+L4+2L5+L01+e+azLp,XS1=px+ecos⁡α+φ4-L3cos⁡α-Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52-L1tan⁡α2,XS2=px+ecos⁡α+φ4-L3cos⁡α-Lpcos⁡α+φ4cos⁡φ5-12-sin⁡α+φ4sin⁡φ52+L1tan⁡α2.Since the available range of the rotation angle is α∈-π/4,π/4 as mentioned above, (32)α=arctan⁡2sin⁡α,1-sin2α,sin⁡α=py-Lpay+eayaz-1+axnz/az-12+nz2L3.


## 2.1.3. Representation of Desired End-Effector Inertial Position Trajectory

Assume that the coordinates of the desired inertial position trajectory of the 5-DOF robot end-effector denoted byxd,yd,zd are specified in the following two forms (see example in Section 6).(a) Implicit Form (33) z d t = f x d t , y d t  t ∈ t 0 , t f ,where fxdt,ydt is a given smooth function and the initial and final positions xdt0,ydt0,zdt0 and xdtf,ydtf,zdtf are given.The implicit form is used in the formulation of the basic multiobjective optimization problem (see Section3.1).(b) Parametric Form (34) x d b = f 1 b , y d b = f 2 b , z d b = f 3 b , b ∈ b 0 ,  b f ,where b is a parameter, fib,i=1,2,3, are given smooth functions, and the initial and final positions xdb0,ydb0,zdb0 and xdbf,ydbf,zdbf are given.The parametric form is used in the formulation of the finite dimensional multiobjective optimization problem (see Section3.2).Given a 5-DOF robot motion trajectoryqtt∈[t0,tf], then the resulting actual inertial position of the end-effector, denoted by xt,yt,ztt∈[t0,tf], is computed by using (28).Assume that the actual inertial position of the end-effectorxt,yt,ztt∈[t0,tf] matches the desired inertial position trajectory specified in implicit form; see (33). Then, it follows that xt,yt,ztt∈[t0,tf] satisfies the following constraint equation:(35)zt=fxt,ytt∈t0,tf,xt0,yt0,zt0=xdt0,ydt0,zdt0,xtf,ytf,ztf=xdtf,ydtf,zdtf.


## 2.2. Dynamic Modelling Using the RNEA

Because of the need for computational efficiency, the robotics community has focused on this problem of the robotic manipulator dynamics. For inverse dynamics, the recursive Newton–Euler algorithm (RNEA) proposed by Luh et al. [43] remains the most important algorithm [44]. The RNEA uses a Newton–Euler formulation of the problem, and this formulation is especially amenable to the development of efficient recursive algorithms for dynamics computations.The RNEA is an algorithm with calculation complexity ofO(n) (where n is the number of degrees of freedom in the mechanism), which is aimed to calculate recursive dynamics of fixed base open-loop robot. The calculation divides into four steps shown as follows, with two steps in each of the two recursions.Step 1.
Calculate the velocity and acceleration of each link in sequence, beginning with the known velocity and acceleration of the fixed base and calculating towards the end link of robot. The velocity of each link in the robot is obtained by the recursive formula(36)vi=vpi+Φiq˙i,v0=0,where vi is the velocity of link i, Φi is the motion matrix of joint i, and q˙i is the vector of joint velocity variables for joint i.The equivalent formula for acceleration is given by differentiating (36):(37)ai=api+Φiq¨i+Φ˙iq˙i,a0=0,where ai is the acceleration of link i and q¨i is the vector of joint acceleration variables.Through initializinga0 to -ag instead of zero (where ag is the gravitational acceleration vector and the gravity is simulated by a fictitious base acceleration), the effect of a uniform gravitational field on robot can be simulated. In this case, ai is not the true acceleration of link i, but the sum of its true acceleration and -ag.Step 2.
Calculate the equation of motion for each link. This step computes the forces required to cause the acceleration calculated in Step1. The equation of motion for link i is shown as(38)fia=Iiai+vi×Iivi,where Ii is the spatial inertia of linki and fia is the net force acting on linki.Step 3.
Calculate the spatial force on each joint. According to Figure3, the force acting on link i is as follows:(39)fia=fie+fi-∑j∈cifj,where fi is the force transmitted across jointi, fie is the sum of all relevant external forces acting on link i, and li is the connected set of link i. Rearranging (38) gives the recursive formula for calculating the joint forces:(40)fi=fia-fie+∑j∈lifj,where i iterates from NB to 1.Figure 3
Forces acting on linki.Step 4.
Calculate the joint force variables,τi. They are given by the equation(41)τi=ΦiTfi.In this section, the dynamics problem (i.e., inverse dynamics problem) of this 5-DOF robot manipulator is solved based on this very efficient RNEA. The velocity and acceleration of each link and the resultant forces on each link are first computed through an outward recursion in turn, starting with the known velocity and acceleration of the fixed base and working towards the tips. A second, inward recursion uses the force balance equations at each link to compute the spatial force across each joint and the value of each joint torque/force variable. Note that, without loss of generality, only the case of the rigid robot manipulator is stressed here; other contributing factors in the dynamic description of the robot manipulator (which may include the dynamics of the actuators, joint and link flexibility, friction, noise, and disturbances) are not considered in this paper.Define thatωij, vij, ω˙ij, v˙ij, fij, and nij are the angular velocity, the linear velocity, the angular acceleration, the linear acceleration, the force, and the moment of the reference frame i with respect to the inertial reference frame and expressed in the reference frame j, respectively. Also define that v˙cij, fcij, and ncij are the linear acceleration of the center of mass of link i, the inertial force, and the inertial moment acting on the center of mass of link i in the reference frame i with respect to the inertial reference frame and expressed in the reference frame j, respectively.The vectorzij is defined as the axis Zi of the reference frame i expressed in the reference frame j. The vector Pij is defined as the origin of the reference frame i with respect to the reference frame j and expressed in the reference frame j. The rotation matrix Rij is defined as the reference frame i with respect to the reference frame j and expressed in the reference frame j.The mass (mi), center of mass (rcii), and moment of inertia about the center of mass (Iici) of link i expressed in reference frame i are listed in Table 4.Table 4
The mass, moment of inertia matrix, and center of mass and of the robot manipulator.
Link Mass (kg) Moment of inertia matrix (kg⋅(mm)2) Center of mass (mm) 1 83.025 5095170.782 - 61.008 1019.065 - 61.008 3291706.841 - 15.858 1019.065 - 15.858 2367692.766 0 0 - 44.182 2 21.333 1475990.573 - 3970.014 - 748196.633 - 3970.014 2366543.574 - 8327.223 - 748196.633 - 8327.223 911968.216 - 304.563 - 1.306 - 278.925 3 8.022 142265.977 - 0.114 0.002 - 0.114 45016.139 46002.096 0.002 46002.096 109393.601 0 - 63.686 - 468.138 4 9.120 119573.680 27.342 - 0.849 27.342 36160.108 - 18452.465 - 0.849 - 18452.465 94076.578 0 - 52.909 - 261.241


### 2.2.1. Outward Recursion

(a) Link 1.As shown in Figures 1 and 2, link 1 is driven in parallel by M1 and M2, and a coupled movement is generated. The angular and linear velocity and acceleration of reference frame 1 representing link 1 with respect to the inertial reference frame and expressed in the inertial reference frame can be directly given as follows:(42)ω10=0,0,α˙T,v10=X˙S1+X˙S22,0,0T,ω˙10=0,0,α¨T,v˙10=X¨S1+X¨S22,0,-gT,where g=9.80665 m/s2, according to (1), yielding(43)α˙=L1X˙S2-X˙S1L12+XS2-XS12,α¨=L1X¨S2-X¨S1L12+XS2-XS12-2L1XS2-XS1X˙S2-X˙S12L12+XS2-XS122.The angular and linear velocity and acceleration of the reference frame 1 and the linear acceleration of the center of mass of link 1 and the inertial force and the inertial moment acting on the center of mass of link 1 in the reference frame 1, with respect to the inertial reference frame and expressed in the reference frame 1, are derived as(44)ω11=R01ω10,(45)v11=R01v10,(46)ω˙11=R01ω˙10,(47)v˙11=R01v˙10,(48)v˙c11=v˙11+ω˙11×rc11+ω11×ω11×rc11,(49)fc11=m1v˙c11,(50)nc11=I1c1ω˙11+ω11×I1c1ω11,where the rotation matrix R01 is given by(51)R01=cos⁡αsin⁡α0-sin⁡αcos⁡α0001.(b) Link 2. The angular and linear velocity and acceleration of the reference frame 2 and the linear acceleration of the center of mass of link 2 and the inertial force and the inertial moment acting on the center of mass of link 2 in the reference frame 2, with respect to the inertial reference frame and expressed in the reference frame 2, are derived as(52)ω22=R12ω11,(53)ω˙22=R12ω˙11,(54)v˙22=R12v˙11+ω˙11×P21+ω11×ω11×P21+2ω22×z12X˙S3+2z1X¨S3,(55)v˙c22=v˙22+ω˙22×rc22+ω22×ω22×rc22,(56)fc22=m2v˙c22,(57)nc22=I2c2ω˙22+ω22×I2c2ω22,where(58)P21=L3,0,XS3+L01T,z12=0,0,1T,R12=100010001.(c) Link 3. The angular and linear velocity and acceleration of the reference frame 3 and the linear acceleration of the center of mass of link 3 and the inertial force and the inertial moment acting on the center of mass of link 3 in the reference frame 3, with respect to the inertial reference frame and expressed in the reference frame 3, are calculated as follows:(59)ω33=R23ω22+z23φ˙4,(60)ω˙33=R23ω˙22+ω22×z23φ˙4+z23φ¨4,(61)v˙33=R23v˙22+ω˙22×P32+ω22×ω22×P32,(62)v˙c33=v˙33+ω˙33×rc33+ω33×ω33×rc33,(63)fc33=m3v˙c33,(64)nc33=I3c3ω˙33+ω33×I3c3ω33,where(65)P32=0,0,L2+L4+2L5T,z23=0,22,22T,R23=sin⁡φ4-cos⁡φ40cos⁡φ42sin⁡φ4222-cos⁡φ42-sin⁡φ4222.(d) Link 4.The angular and linear velocity and acceleration of the reference frame 4 and the linear acceleration of the center of mass of link 4 and the inertial force and the inertial moment acting on the center of mass of link 4 in the reference frame 4, with respect to the inertial reference frame and expressed in the reference frame 4, are given as follows:(66)ω44=R34ω33+z34φ˙5,(67)ω˙44=R34ω˙33+R34ω33×z34φ˙5+z34φ¨5,(68)v˙44=R34v˙33+ω˙33×P43+ω33×ω33×P43,(69)v˙c44=v˙44+ω˙44×rc44+ω44×ω44×rc44,(70)fc44=m4v˙c44,(71)nc44=I4c4ω˙44+ω44×I4c4ω44,where(72)P43=0,0,2eT,z34=0,22,22T,R34=-cos⁡φ5-sin⁡φ50sin⁡φ52-cos⁡φ5222-sin⁡φ52cos⁡φ5222.


### 2.2.2. Inward Recursion

For each link of robot manipulator, except the gravitational force, the forces and moments acting one link belong to internal forces and moments acting between the connected links; that is, the external forces and moments relationships between connected links are forces/moments and reaction forces/moments. A diagram is given for displaying the external forces and resultant forces (shown in Figure4). External forces and moments acting on link i are shown in Figure 4(a). Here, the force and moment of link i-1 acting on link i are resultant fii and nii, respectively; also, the force and moment of link i+1 acting on link i are resultant -fi+1i and -ni+1i, respectively; mig is the gravitational force of link i (where mi is the mass of link i and g is gravitational acceleration vector). The resultant force and moment at the center of mass of link i caused by these external forces and moments acting on link i based on static balancing conditions are fcii and ncii (shown in Figure 4(b)), respectively.Figure 4
External forces and resultant forces of linki: (a) external forces and moments acting on link i and (b) resultant force and moment at the center of mass of link i.
(a) (b)(a) Link 4.When a robot performs a manipulation task (e.g., polishing a surface), the robot establishes physical contact with external environment; the relevant external forces and/or moments will act on the end-effector. In order to express this influence in the dynamic equations of motion, suppose that the resultant of all relevant external forces and moments applied on the origin of the end-effector reference frame 5 is ftool and ntool, respectively. Thus, the force and moment of link 4 applied on the origin of the reference frame 5 are f55=-ftool and n55=-ntool, respectively. The payload mass mp can be entered as an external force ftool.The force and the moment of the reference frame 4 with respect to the inertial reference frame and expressed in the reference frame 4 and the required joint actuator torque of joint 5 are derived as(73)f44=R54f55+fc44,(74)n44=R54n55+nc44+rc44×fc44+P54×R54f55,(75)τ5=4z3Tn44,where(76)P54=0,0,LpT,R54=100010001.Note that, for link 4, the inertial forcefc44 and the inertial moment nc44 (which are expressed in (70) and (71), resp.) are represented as the term nc44+rc44×fc44 in (74) in order to apply the influence of the center of mass of link 4 on the origin of the reference frame 4. Other influences of the center of mass of link i(i=1,2,3) have the same representation forms.(b) Link 3. The force and the moment of the reference frame 3 with respect to the inertial reference frame and expressed in the reference frame 3 and the required joint actuator torque of joint 4 are obtained as(77)f33=R43f44+fc33,(78)n33=4R3Tn44+nc33+rc33×fc33+P43×4R3Tf44,(79)τ4=3z2T3n3.(c) Link 2. The force and the moment of the reference frame 2 with respect to the inertial reference frame and expressed in the reference frame 2 and the required joint actuator torque of joint 3 are expressed as follows:(80)f22=R32f33+fc22,(81)n22=R2T3n33+nc22+rc22×fc22+P32×R2T3f33,(82)F3=2z1Tf22.(d) Link 1. The force and the moment of the reference frame 1 with respect to the inertial reference frame and expressed in the reference frame 1 are given as follows:(83)f11=R21f22+fc11,(84)n11=R21n22+nc11+rc11×fc11+P21×R21f22.The interaction forces between link 1 and screw joints (consisting of ball screws) are shown in Figure5. Combining Newton’s and Euler’s equations for the link 1 leads to(85)L1F2-F12cos2⁡α=1z0TR10n11F2+F1=1x0TR10f11,where z01=0,0,1T and x01=1,0,0T.Figure 5
The interaction forces: (a) screw joints and (b) link 1.
(a) (b)The required joint actuator forces of joint 1 and joint 2 are derived by (85); that is,(86)F1=x10TR10f112-z10TR10n11cos2⁡αL1,(87)F2=x10TR10f112+z10TR10n11cos2⁡αL1.Ignoring transmission power loss, the resulting five applied motor torques (while the five reaction torques are equal in magnitude and opposite in direction) can be obtained as(88)TM1=F1sx2π,TM2=F2sx2π,TM3=F3sz2π,TM4=τ4,TM5=τ5.Thus, given a 5-DOF robot trajectoryqt,q˙t,q¨tt∈[t0,tf], then the applied forces and torques U=F1F2F3τ4τ5T required to produce the above-mentioned trajectory are computed as follows.Step 1.
Calculate the velocity and acceleration of each link in turn, starting with the known velocity and acceleration of the fixed base, and working towards the end. They are computed by (44)–(47), (52)–(54), (59)–(61), and (66)–(68).Step 2.
Calculate the equation of motion for each link. They are determined by (50), (57), (64), and (71).Step 3.
Calculate the spatial force on each joint. They are given by (74), (78), (81), and (84).Step 4.
Calculate the joint force variables (i.e., the applied forces and torquesU=F1F2F3τ4τ5T). They are obtained by (75), (79), (82), (86), and (87).


## 2.2.1. Outward Recursion

(a) Link 1.As shown in Figures 1 and 2, link 1 is driven in parallel by M1 and M2, and a coupled movement is generated. The angular and linear velocity and acceleration of reference frame 1 representing link 1 with respect to the inertial reference frame and expressed in the inertial reference frame can be directly given as follows:(42)ω10=0,0,α˙T,v10=X˙S1+X˙S22,0,0T,ω˙10=0,0,α¨T,v˙10=X¨S1+X¨S22,0,-gT,where g=9.80665 m/s2, according to (1), yielding(43)α˙=L1X˙S2-X˙S1L12+XS2-XS12,α¨=L1X¨S2-X¨S1L12+XS2-XS12-2L1XS2-XS1X˙S2-X˙S12L12+XS2-XS122.The angular and linear velocity and acceleration of the reference frame 1 and the linear acceleration of the center of mass of link 1 and the inertial force and the inertial moment acting on the center of mass of link 1 in the reference frame 1, with respect to the inertial reference frame and expressed in the reference frame 1, are derived as(44)ω11=R01ω10,(45)v11=R01v10,(46)ω˙11=R01ω˙10,(47)v˙11=R01v˙10,(48)v˙c11=v˙11+ω˙11×rc11+ω11×ω11×rc11,(49)fc11=m1v˙c11,(50)nc11=I1c1ω˙11+ω11×I1c1ω11,where the rotation matrix R01 is given by(51)R01=cos⁡αsin⁡α0-sin⁡αcos⁡α0001.(b) Link 2. The angular and linear velocity and acceleration of the reference frame 2 and the linear acceleration of the center of mass of link 2 and the inertial force and the inertial moment acting on the center of mass of link 2 in the reference frame 2, with respect to the inertial reference frame and expressed in the reference frame 2, are derived as(52)ω22=R12ω11,(53)ω˙22=R12ω˙11,(54)v˙22=R12v˙11+ω˙11×P21+ω11×ω11×P21+2ω22×z12X˙S3+2z1X¨S3,(55)v˙c22=v˙22+ω˙22×rc22+ω22×ω22×rc22,(56)fc22=m2v˙c22,(57)nc22=I2c2ω˙22+ω22×I2c2ω22,where(58)P21=L3,0,XS3+L01T,z12=0,0,1T,R12=100010001.(c) Link 3. The angular and linear velocity and acceleration of the reference frame 3 and the linear acceleration of the center of mass of link 3 and the inertial force and the inertial moment acting on the center of mass of link 3 in the reference frame 3, with respect to the inertial reference frame and expressed in the reference frame 3, are calculated as follows:(59)ω33=R23ω22+z23φ˙4,(60)ω˙33=R23ω˙22+ω22×z23φ˙4+z23φ¨4,(61)v˙33=R23v˙22+ω˙22×P32+ω22×ω22×P32,(62)v˙c33=v˙33+ω˙33×rc33+ω33×ω33×rc33,(63)fc33=m3v˙c33,(64)nc33=I3c3ω˙33+ω33×I3c3ω33,where(65)P32=0,0,L2+L4+2L5T,z23=0,22,22T,R23=sin⁡φ4-cos⁡φ40cos⁡φ42sin⁡φ4222-cos⁡φ42-sin⁡φ4222.(d) Link 4.The angular and linear velocity and acceleration of the reference frame 4 and the linear acceleration of the center of mass of link 4 and the inertial force and the inertial moment acting on the center of mass of link 4 in the reference frame 4, with respect to the inertial reference frame and expressed in the reference frame 4, are given as follows:(66)ω44=R34ω33+z34φ˙5,(67)ω˙44=R34ω˙33+R34ω33×z34φ˙5+z34φ¨5,(68)v˙44=R34v˙33+ω˙33×P43+ω33×ω33×P43,(69)v˙c44=v˙44+ω˙44×rc44+ω44×ω44×rc44,(70)fc44=m4v˙c44,(71)nc44=I4c4ω˙44+ω44×I4c4ω44,where(72)P43=0,0,2eT,z34=0,22,22T,R34=-cos⁡φ5-sin⁡φ50sin⁡φ52-cos⁡φ5222-sin⁡φ52cos⁡φ5222.


## 2.2.2. Inward Recursion

For each link of robot manipulator, except the gravitational force, the forces and moments acting one link belong to internal forces and moments acting between the connected links; that is, the external forces and moments relationships between connected links are forces/moments and reaction forces/moments. A diagram is given for displaying the external forces and resultant forces (shown in Figure4). External forces and moments acting on link i are shown in Figure 4(a). Here, the force and moment of link i-1 acting on link i are resultant fii and nii, respectively; also, the force and moment of link i+1 acting on link i are resultant -fi+1i and -ni+1i, respectively; mig is the gravitational force of link i (where mi is the mass of link i and g is gravitational acceleration vector). The resultant force and moment at the center of mass of link i caused by these external forces and moments acting on link i based on static balancing conditions are fcii and ncii (shown in Figure 4(b)), respectively.Figure 4
External forces and resultant forces of linki: (a) external forces and moments acting on link i and (b) resultant force and moment at the center of mass of link i.
(a) (b)(a) Link 4.When a robot performs a manipulation task (e.g., polishing a surface), the robot establishes physical contact with external environment; the relevant external forces and/or moments will act on the end-effector. In order to express this influence in the dynamic equations of motion, suppose that the resultant of all relevant external forces and moments applied on the origin of the end-effector reference frame 5 is ftool and ntool, respectively. Thus, the force and moment of link 4 applied on the origin of the reference frame 5 are f55=-ftool and n55=-ntool, respectively. The payload mass mp can be entered as an external force ftool.The force and the moment of the reference frame 4 with respect to the inertial reference frame and expressed in the reference frame 4 and the required joint actuator torque of joint 5 are derived as(73)f44=R54f55+fc44,(74)n44=R54n55+nc44+rc44×fc44+P54×R54f55,(75)τ5=4z3Tn44,where(76)P54=0,0,LpT,R54=100010001.Note that, for link 4, the inertial forcefc44 and the inertial moment nc44 (which are expressed in (70) and (71), resp.) are represented as the term nc44+rc44×fc44 in (74) in order to apply the influence of the center of mass of link 4 on the origin of the reference frame 4. Other influences of the center of mass of link i(i=1,2,3) have the same representation forms.(b) Link 3. The force and the moment of the reference frame 3 with respect to the inertial reference frame and expressed in the reference frame 3 and the required joint actuator torque of joint 4 are obtained as(77)f33=R43f44+fc33,(78)n33=4R3Tn44+nc33+rc33×fc33+P43×4R3Tf44,(79)τ4=3z2T3n3.(c) Link 2. The force and the moment of the reference frame 2 with respect to the inertial reference frame and expressed in the reference frame 2 and the required joint actuator torque of joint 3 are expressed as follows:(80)f22=R32f33+fc22,(81)n22=R2T3n33+nc22+rc22×fc22+P32×R2T3f33,(82)F3=2z1Tf22.(d) Link 1. The force and the moment of the reference frame 1 with respect to the inertial reference frame and expressed in the reference frame 1 are given as follows:(83)f11=R21f22+fc11,(84)n11=R21n22+nc11+rc11×fc11+P21×R21f22.The interaction forces between link 1 and screw joints (consisting of ball screws) are shown in Figure5. Combining Newton’s and Euler’s equations for the link 1 leads to(85)L1F2-F12cos2⁡α=1z0TR10n11F2+F1=1x0TR10f11,where z01=0,0,1T and x01=1,0,0T.Figure 5
The interaction forces: (a) screw joints and (b) link 1.
(a) (b)The required joint actuator forces of joint 1 and joint 2 are derived by (85); that is,(86)F1=x10TR10f112-z10TR10n11cos2⁡αL1,(87)F2=x10TR10f112+z10TR10n11cos2⁡αL1.Ignoring transmission power loss, the resulting five applied motor torques (while the five reaction torques are equal in magnitude and opposite in direction) can be obtained as(88)TM1=F1sx2π,TM2=F2sx2π,TM3=F3sz2π,TM4=τ4,TM5=τ5.Thus, given a 5-DOF robot trajectoryqt,q˙t,q¨tt∈[t0,tf], then the applied forces and torques U=F1F2F3τ4τ5T required to produce the above-mentioned trajectory are computed as follows.Step 1.
Calculate the velocity and acceleration of each link in turn, starting with the known velocity and acceleration of the fixed base, and working towards the end. They are computed by (44)–(47), (52)–(54), (59)–(61), and (66)–(68).Step 2.
Calculate the equation of motion for each link. They are determined by (50), (57), (64), and (71).Step 3.
Calculate the spatial force on each joint. They are given by (74), (78), (81), and (84).Step 4.
Calculate the joint force variables (i.e., the applied forces and torquesU=F1F2F3τ4τ5T). They are obtained by (75), (79), (82), (86), and (87).


## 2.3. Dynamic Modelling Using the Lagrange Equations

Besides the RNEA, the robot dynamic equations of motion can also be derived by the Lagrange equations. The dynamic equations can be deduced by calculating the kinetic and potential energies of the links and by defining the Lagrangian and by differentiating the Lagrangian equation with respect to the joint variables [45].


### 2.3.1. Kinetic Energy

As mentioned above, the link 1 is driven in parallel byM1 and M2, and a coupled movement is generated. In order to separately express this coupled movement, an addition virtual link with zero mass and zero length is introduced between the base and link 1 to produce a translational motion. Thus, a translational motion of the virtual link along the axis X1 and a rotation motion of the link 1 about the axis Z1 can be decoupled from this coupled movement. The corresponding transformation matrix 0T1 can be represented as(89)T10=A1A2,A1=100d1010000100001,A2=cos⁡ϕ2-sin⁡ϕ200sin⁡ϕ2cos⁡ϕ20000100001,where(90)d1=XS1+XS22,ϕ2=α.Other transformation matricesi-1Ti for the link i(i=2,3,4) are represented by Aj(j=3,4,5), respectively; that is,(91)Aj=Tii-1i=2,3,4j=i+1,where the joint variables are given as follows:(92)d3=XS3+L01,ϕ4=-π2+φ4,ϕ5=π+φ5.The transformation between the reference framei and the inertial reference frame can be written as(93)Ti0=T10T21⋯Tii-1=A1A2⋯Aii=1,2,…,5.Using local generalized coordinatesψ=d1,ϕ2,d3,ϕ4,ϕ5T to represent these joint variables (d1 and d3 for prismatic joints and ϕ1, ϕ4 and ϕ5 for revolute joints), the derivative of Ai matrix for a revolute joint or a prismatic joint can be expressed as(94)∂Ai∂ψi=ρiAii=1,2,…,5,where(95)ρirevolute=0-100100000000000,ρiprismatic=0000000000010000.Extending the same differentiation principle to the0Ti matrix of (93) with multiple joint variables differentiated with respect to only one variable qj gives(96)Γij=∂Ti0∂ψj=∂A1A2⋯Aj⋯Ai∂ψj=A1A2⋯QjAj⋯Aij≤i≤5.Higher-order derivatives can be formulated similarly from(97)Γijk=∂Γij∂ψk.Usingri to represent a point on any link i of the robot relative to frame i, the position of the point can expressed by premultiplying the vector with the transformation matrix representing its frame as follows:(98)pi=Ti0ri.Differentiating (98) with respect to all the local variables ψj yields the velocity of the point:(99)Vi=ddtTi0ri=∑j=1i∂Ti0∂ψjdψjdtri=∑j=1iΓijdψjdtri.The kinetic energy of an element of massmi on a link is(100)dKi=12Trace∑p=1iΓipdψpdtri∑r=1iΓirdψrdtriTdmi,where the mathematical symbol “Trace” is the trace of a matrix. Integrating this equation and rearranging terms yield the total kinetic energy:(101)Ki=∫dKi=12Trace∑p=1i∑r=1iΓip∫ririTdmiΓirTψ˙pψ˙r.The Pseudo Inertia Matrix, representing the∫ririTdmi terms, can be written as(102)Ji=-Ixx+Iyy+Izzi2IixyIixzmixlciIixyIxx-Iyy+Izzi2IiyzmiylciIixzIiyzIxx+Iyy-Izzi2mizlcimixlcimiylcimizlcimi,where rlci=xlci,ylci,zlciT is the location of the center of mass of the link i relative to the frame representing the link. For the local generalized variables ψ=d1,ϕ2,d3,ϕ4,ϕ5T, the corresponding Pseudo Inertia Matrices are J1, J2, J3, J4, and J5, respectively. The Pseudo Inertia Matrices and the location of the center of mass of the studied robot manipulator are listed in Table 5.Table 5
The mass (mi), Pseudo Inertia Matrix (Ji), and the location of the center of mass (rlci).
Number m i J i r l c i 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 2 83.025 282114.412 - 61.008 1019.065 0 - 61.008 2085578.353 - 15.858 0 1019.065 - 15.858 3009592.428 - 3668.210 0 0 - 3668.210 83.025 0 0 - 44.182 1 3 21.333 901260.608 - 3970.014 - 748196.633 - 6497.242 - 3970.014 2085578.353 - 8327.223 - 27.860 - 748196.633 - 8327.223 3009592.428 - 3668.210 - 6497.242 - 27.860 - 3668.210 21.333 - 304.563 - 1.306 - 278.925 1 4 8.022 6071.881 - 0.114 0.002 0 - 0.114 103321.719 46002.096 - 510.889 0.002 46002.096 38944.257 - 3755.403 0 - 510.889 - 3755.403 8.022 0 - 63.686 - 468.138 1 5 9.120 5331.503 27.342 - 0.849 0.501 27.342 88745.074 - 18452.465 - 482.530 - 0.849 - 18452.465 30828.605 - 2382.517 0.501 - 482.530 - 2382.517 9.120 0 - 52.909 - 261.241 1Substituting (102) into (101) gives the final form for kinetic energy of the studied robot manipulator (here, the kinetic energy of the actuators is not stressed in this paper):(103)K=12∑i=15∑p=1i∑r=1iTraceΓipJiΓirTψ˙pψ˙r.


### 2.3.2. Potential Energy

The potential energy of the system is the sum of the potential energies of each link and can be written as(104)P=∑i=15Pi=∑i=15-migT0Tirlci,where gT=[gxgygz0] is the gravity matrix. Notice that the values in the gravity matrix are dependent on the orientation of the reference frame.


### 2.3.3. The Lagrangian

The Lagrangian of the robot manipulator is then expressed as(105)L=K-P=12∑i=15∑p=1i∑r=1iTraceΓipJiΓirTψ˙pψ˙r-∑i=15-migT0Tirlci.


### 2.3.4. The Lagrangian Dynamics for the Robot’s Equations of Motion

The Lagrangian of the robot manipulator can be differentiated for local generalized coordinatesψ=d1,ϕ2,d3,ϕ4,ϕ5T in order to form the dynamic equations of motion:(106)T=∂∂t∂L∂ψ˙-∂L∂ψ.The final equations of motion for all local generalized variablesψ=d1,ϕ2,d3,ϕ4,ϕ5T can be derived as follows:(107)Ti=∑j=15Dijψ¨j+∑j=15∑k=15Dijkψ˙jψ˙k+Dii=1,2,…,5,where(108)Dij=∑p=max⁡i,j5TraceΓpjJpΓpiT,Dijk=∑p=max⁡i,j,k5TraceΓpjkJpΓpiT,Di=∑p=i5-mpgTΓpirlcp.The following derivations of transformation relationship, between equations of motionU=F1,F2,F3,τ4,τ5T (represented by the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T) and equations of motion U=F1,F2,F3,τ4,τ5T (represented by the local generalized variables ψ=d1,ϕ2,d3,ϕ4,ϕ5T), can be yielded as follows.The relationship between forcesF1 and F2, respectively, produced by the generalized coordinates XS1 and XS2 and T1 and T2, respectively, produced by the virtual generalized coordinate d1 and ϕ2 can be derived using the principle of virtual work and (90) as(109)F1δXS1+F2δXS2=T1δd1+T2δϕ2δd1=δXS1+δXS22δϕ2=cos2⁡αδXS2-δXS1L1.Based on (6) and (109), it yields(110)F1=T12-T2L1XS2-XS12+L12,F2=T12+T2L1XS2-XS12+L12.Similarly, based on (92), it gives(111)F3=T3,τ4=T4,τ5=T5.From the above derivations in this section, the dynamic equations of motion represented by the local generalized variablesψ=d1,ϕ2,d3,ϕ4,ϕ5T and the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T for the studied 5-DOF robot manipulator are obtained, respectively.Silver compared the computational efficiency between Newton–Euler and Lagrangian formulations for manipulator dynamics and proved that they are indeed equivalent to each other [46]. There is in fact no fundamental difference in computational efficiency between two. The former is significantly more efficient than the latter due to two factors: the recursive structure of the computation and the representation chosen for the rotational dynamics. Namely, the former works directly with Newton’s and Euler’s equations for a rigid body, which are contained within the spatial equation of motion. This formulation is especially amenable to the development of efficient recursive algorithms for dynamics computations. The latter only considers the total robot energy and proceeds via the Lagrangian of the robot mechanism; then the dynamic equations of motion are developed using Lagrange’s equation for each defined generalized coordinate.


## 2.3.1. Kinetic Energy

As mentioned above, the link 1 is driven in parallel byM1 and M2, and a coupled movement is generated. In order to separately express this coupled movement, an addition virtual link with zero mass and zero length is introduced between the base and link 1 to produce a translational motion. Thus, a translational motion of the virtual link along the axis X1 and a rotation motion of the link 1 about the axis Z1 can be decoupled from this coupled movement. The corresponding transformation matrix 0T1 can be represented as(89)T10=A1A2,A1=100d1010000100001,A2=cos⁡ϕ2-sin⁡ϕ200sin⁡ϕ2cos⁡ϕ20000100001,where(90)d1=XS1+XS22,ϕ2=α.Other transformation matricesi-1Ti for the link i(i=2,3,4) are represented by Aj(j=3,4,5), respectively; that is,(91)Aj=Tii-1i=2,3,4j=i+1,where the joint variables are given as follows:(92)d3=XS3+L01,ϕ4=-π2+φ4,ϕ5=π+φ5.The transformation between the reference framei and the inertial reference frame can be written as(93)Ti0=T10T21⋯Tii-1=A1A2⋯Aii=1,2,…,5.Using local generalized coordinatesψ=d1,ϕ2,d3,ϕ4,ϕ5T to represent these joint variables (d1 and d3 for prismatic joints and ϕ1, ϕ4 and ϕ5 for revolute joints), the derivative of Ai matrix for a revolute joint or a prismatic joint can be expressed as(94)∂Ai∂ψi=ρiAii=1,2,…,5,where(95)ρirevolute=0-100100000000000,ρiprismatic=0000000000010000.Extending the same differentiation principle to the0Ti matrix of (93) with multiple joint variables differentiated with respect to only one variable qj gives(96)Γij=∂Ti0∂ψj=∂A1A2⋯Aj⋯Ai∂ψj=A1A2⋯QjAj⋯Aij≤i≤5.Higher-order derivatives can be formulated similarly from(97)Γijk=∂Γij∂ψk.Usingri to represent a point on any link i of the robot relative to frame i, the position of the point can expressed by premultiplying the vector with the transformation matrix representing its frame as follows:(98)pi=Ti0ri.Differentiating (98) with respect to all the local variables ψj yields the velocity of the point:(99)Vi=ddtTi0ri=∑j=1i∂Ti0∂ψjdψjdtri=∑j=1iΓijdψjdtri.The kinetic energy of an element of massmi on a link is(100)dKi=12Trace∑p=1iΓipdψpdtri∑r=1iΓirdψrdtriTdmi,where the mathematical symbol “Trace” is the trace of a matrix. Integrating this equation and rearranging terms yield the total kinetic energy:(101)Ki=∫dKi=12Trace∑p=1i∑r=1iΓip∫ririTdmiΓirTψ˙pψ˙r.The Pseudo Inertia Matrix, representing the∫ririTdmi terms, can be written as(102)Ji=-Ixx+Iyy+Izzi2IixyIixzmixlciIixyIxx-Iyy+Izzi2IiyzmiylciIixzIiyzIxx+Iyy-Izzi2mizlcimixlcimiylcimizlcimi,where rlci=xlci,ylci,zlciT is the location of the center of mass of the link i relative to the frame representing the link. For the local generalized variables ψ=d1,ϕ2,d3,ϕ4,ϕ5T, the corresponding Pseudo Inertia Matrices are J1, J2, J3, J4, and J5, respectively. The Pseudo Inertia Matrices and the location of the center of mass of the studied robot manipulator are listed in Table 5.Table 5
The mass (mi), Pseudo Inertia Matrix (Ji), and the location of the center of mass (rlci).
Number m i J i r l c i 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 2 83.025 282114.412 - 61.008 1019.065 0 - 61.008 2085578.353 - 15.858 0 1019.065 - 15.858 3009592.428 - 3668.210 0 0 - 3668.210 83.025 0 0 - 44.182 1 3 21.333 901260.608 - 3970.014 - 748196.633 - 6497.242 - 3970.014 2085578.353 - 8327.223 - 27.860 - 748196.633 - 8327.223 3009592.428 - 3668.210 - 6497.242 - 27.860 - 3668.210 21.333 - 304.563 - 1.306 - 278.925 1 4 8.022 6071.881 - 0.114 0.002 0 - 0.114 103321.719 46002.096 - 510.889 0.002 46002.096 38944.257 - 3755.403 0 - 510.889 - 3755.403 8.022 0 - 63.686 - 468.138 1 5 9.120 5331.503 27.342 - 0.849 0.501 27.342 88745.074 - 18452.465 - 482.530 - 0.849 - 18452.465 30828.605 - 2382.517 0.501 - 482.530 - 2382.517 9.120 0 - 52.909 - 261.241 1Substituting (102) into (101) gives the final form for kinetic energy of the studied robot manipulator (here, the kinetic energy of the actuators is not stressed in this paper):(103)K=12∑i=15∑p=1i∑r=1iTraceΓipJiΓirTψ˙pψ˙r.


## 2.3.2. Potential Energy

The potential energy of the system is the sum of the potential energies of each link and can be written as(104)P=∑i=15Pi=∑i=15-migT0Tirlci,where gT=[gxgygz0] is the gravity matrix. Notice that the values in the gravity matrix are dependent on the orientation of the reference frame.


## 2.3.3. The Lagrangian

The Lagrangian of the robot manipulator is then expressed as(105)L=K-P=12∑i=15∑p=1i∑r=1iTraceΓipJiΓirTψ˙pψ˙r-∑i=15-migT0Tirlci.


## 2.3.4. The Lagrangian Dynamics for the Robot’s Equations of Motion

The Lagrangian of the robot manipulator can be differentiated for local generalized coordinatesψ=d1,ϕ2,d3,ϕ4,ϕ5T in order to form the dynamic equations of motion:(106)T=∂∂t∂L∂ψ˙-∂L∂ψ.The final equations of motion for all local generalized variablesψ=d1,ϕ2,d3,ϕ4,ϕ5T can be derived as follows:(107)Ti=∑j=15Dijψ¨j+∑j=15∑k=15Dijkψ˙jψ˙k+Dii=1,2,…,5,where(108)Dij=∑p=max⁡i,j5TraceΓpjJpΓpiT,Dijk=∑p=max⁡i,j,k5TraceΓpjkJpΓpiT,Di=∑p=i5-mpgTΓpirlcp.The following derivations of transformation relationship, between equations of motionU=F1,F2,F3,τ4,τ5T (represented by the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T) and equations of motion U=F1,F2,F3,τ4,τ5T (represented by the local generalized variables ψ=d1,ϕ2,d3,ϕ4,ϕ5T), can be yielded as follows.The relationship between forcesF1 and F2, respectively, produced by the generalized coordinates XS1 and XS2 and T1 and T2, respectively, produced by the virtual generalized coordinate d1 and ϕ2 can be derived using the principle of virtual work and (90) as(109)F1δXS1+F2δXS2=T1δd1+T2δϕ2δd1=δXS1+δXS22δϕ2=cos2⁡αδXS2-δXS1L1.Based on (6) and (109), it yields(110)F1=T12-T2L1XS2-XS12+L12,F2=T12+T2L1XS2-XS12+L12.Similarly, based on (92), it gives(111)F3=T3,τ4=T4,τ5=T5.From the above derivations in this section, the dynamic equations of motion represented by the local generalized variablesψ=d1,ϕ2,d3,ϕ4,ϕ5T and the generalized coordinates q=XS1,XS2,XS3,φ4,φ5T for the studied 5-DOF robot manipulator are obtained, respectively.Silver compared the computational efficiency between Newton–Euler and Lagrangian formulations for manipulator dynamics and proved that they are indeed equivalent to each other [46]. There is in fact no fundamental difference in computational efficiency between two. The former is significantly more efficient than the latter due to two factors: the recursive structure of the computation and the representation chosen for the rotational dynamics. Namely, the former works directly with Newton’s and Euler’s equations for a rigid body, which are contained within the spatial equation of motion. This formulation is especially amenable to the development of efficient recursive algorithms for dynamics computations. The latter only considers the total robot energy and proceeds via the Lagrangian of the robot mechanism; then the dynamic equations of motion are developed using Lagrange’s equation for each defined generalized coordinate.


## 3. Problem Formulation

For a given prespecified trajectory in the task space of this 5-DOF robot manipulator in this paper, the aim is to optimize a trajectory and calculate the maximum DLCC. This can be formulated as an optimization problem which optimizes four objective functions considering the robot manipulator physical constraints, payload constraint, and actuator limitations.


### 3.1. Cost Function and Constraints

In this paper, the cost function is the weighted sum of four objective functions representing traveling time, total energy involved in the motion of the robot manipulator, joint jerks, and joint acceleration, respectively. The cost function is considered as the evaluation criterion for the optimal trajectory planning problem.In practice, the results of the multiobjective optimization problems are influenced by the values of the weighting coefficients. Hence, appropriate values are required to characterize the importance of the corresponding decision criteria for the trajectory planning applications. Multiple objectives can be combined into a scalar cost function via a weight vector. Weights may be assigned through the direct assignment, the eigenvector method, the empty method, the minimal information method, the random determination, or the adaptation determination. In this paper, it is considered that minimizing operation time (minimum traveling time) and saving energy (minimizing total energy involved in the motion) take the highest priority over others. The next priority is applied for reducing vibrations of robot manipulator joints (minimizing joint jerks and acceleration). Thus, a corresponding vector of normalized weights,w1=0.3, w2=0.3, w3=0.2 and w4=0.2, is assigned for the objective functions in the optimization problem.Significant differences may be made in the magnitude of each function when the objective functions are simply weighted and used to produce a single cost function, resulting in a dominating effort that is mainly caused by the function corresponding to the largest magnitude. In particular, the larger the magnitude of an objective function, the higher the influence on the cost function. Hence, a feasible and efficient means will be needed to deal with this problem effected by the large magnitude of the objective function. One possibility of avoiding this problem is to make an adequate normalization of the objective functions and formulate the same magnitude of objective functions. It can have a roughly equivalent influence on the objective functions for the cost function. Also normalizing parameters make all objective functions as unitless functions. In this paper, the normalized weighted objective functions method is used to balance the influence on four objective functions and select the best found solution. More details on this method can be referred to [47, 48]. The values of ξ1=1, ξ2=1000, ξ3=10, and ξ4=1 are taken as the normalizing parameters of the objective functions z1, z2, z3, and z4.As mentioned above, joint variablesq=[XS1XS2XS3φ4φ5]T of this robot manipulator form a vector of generalized coordinates. Likewise, the variables q˙, q¨, q⃛, and U=[F1F2F3τ4τ5]T are generalized velocities, acceleration, jerks, and forces.Given the following date, (a) the initial timet0t0≥0; the final time tf which is unknown and must be determined (see below); (b) the coordinates of the desired inertial position trajectory of the 5-DOF robot end-effector specified in two forms (see Section 2.1.3), the resulting desired initial and final conditions of the generalized coordinates qdt0 and qdtf are calculated by using (29)–(32); set qt0=qdt0 and qtf=qdtf; (c) the initial and final conditions are q˙t0, q¨t0, q⃛t0, q˙tf, q¨tf, and q⃛tf; thus, the basic multiobjective optimization problem is stated as follows.Compute the final timetf, the payload mass mp, and a smooth 5-DOF robot motion trajectory qt,q˙t,q¨t,q⃛tt∈[t0,tf] satisfying the above-mentioned initial and final conditions so as to minimize the cost function Fc given by(112)Fc=w1z1ξ1+w2z2ξ2+w3z3ξ3+w4z4ξ4,Where one has the following:Total traveling time between initial and final configurations:(113)z1=tf-t0.Total energy involved in the motion of the robot manipulator:(114)z2=∫t0tf∑j=15UjTtq˙jt2dt.Integral of joint jerks squared:(115)z3=∫t0tf∑j=15q⃛jt2dt.Integral of joint acceleration squared:(116)z4=∫t0tf∑j=15q¨jt2dt.subject to the following constraints:The final time constraint:(117)tf≥t0.The displacement constraint:(118)QCmin_j≤qjt≤QCmax_jj=1,…,5,t∈t0,tf.The velocity constraint:(119)q˙jt≤VCjj=1,…,5,t∈t0,tf.The acceleration constraint:(120)q¨jt≤WCjj=1,…,5,t∈t0,tf.The jerk constraint:(121)q⃛jt≤JCjj=1,…,5,t∈t0,tf.The force/torque constraint:(122)Ujt≤UCjj=1,…,5,t∈t0,tf;that is, F1t≤UC1,F2t≤UC2,F3t≤UC3,τ4t≤UC4,τ5t≤UC5,t∈t0,tf.The payload constraint:(123)mp_min≤mp≤mp_max.The kinematic constraints: that is, (29)–(32) (which are specific specified in Section 2.1).The dynamic constraints: that is, (75), (79), (82), and (86)–(88) (which are specific specified in Section 2.2 are also considered for the multiobjective optimization problem).Here,QCmin_j and QCmax_j are thejth joint lower and upper bounds of displacement, respectively. VCj, WCj, JCj, and UCj are thejth joint upper bounds of velocity, acceleration, jerk, and force/torque, respectively. mp_min and mp_max are the minimum and maximum allowable load corresponding to the DLCC of the robot manipulator, respectively.


### 3.2. Finite Dimensional 5-DOF Robot Motion Trajectory Representation

The cubic spline in trajectory planning is very useful, because the generated trajectories have continuous values of the acceleration; also, unlike higher-order polynomials, cubic splines would not generate problems such as excessive oscillations and overshoot between any pair of reference points.In this paper, cubic splines are applied to represent the trajectory of the joint space, given that the desired inertial position trajectory of the 5-DOF robot end-effector is specified in parametric form (see Section2.1.3). Also, given a positive integer m0, select an increasing sequence of m0 parameter values b1,b2,…,bm0 lying in the parameter interval b0,bf such that(124)b0=b1<b2<⋯<bm0=bf.Thus,m0 via-points points are obtained on the desired inertial position trajectory of the 5-DOF robot end-effector with coordinates(125)xdbi,ydbi,zdbii=1,2,…,m0.By using the inverse kinematics of the 5-DOF robot (see (29)–(32)), m0 via-points are obtained on the desired trajectory of the 5-DOF robot generalized coordinates qdt at a sequence of m0 increasing time instants indexed as follows:(126)t0=t1<t3<⋯<tm-2<tm,where the initial time t0 is given and m=m0+2.An additional two via-pointsqdt2 and qdtm-1, corresponding to times t2 and tm-1, are added in order to accommodate the specified initial and final conditions q˙t1, q¨t1, q˙tm, and q¨tm (see below).Thus, a total ofm via-points on the desired trajectory of the 5-DOF robot generalized coordinates qdt are known and are denoted as follows:(127)qj,i=qdjtij=1,2,…,5,i=1,2,…,m.What is not known and must be determined is the sequence ofm-1 increasing time instants,(128)t0=t1<t2<t3<⋯<tm-2<tm-1<tm.The sequence of time instantsti,i=1,2,…,m, defines m-1 consecutive time intervals Ii given by(129)Ii=ti,ti+1,i=1,2,…,m-1.Let hi denote the length of the time interval Ii; that is, hi=ti+1-ti,i=1,2,…,m-1.Given the above-mentionedm via-points and the time-interval lengths hi,i=1,2,…,m-1, then the actual motion trajectory of the 5-DOF robot is constructed by using a cubic spline segment to connect every pair of consecutive via-points; see (127).In this section, a methodology is proposed for computing the unknown time-interval lengthshi,i=1,2,…,m-1, such that the resulting actual motion trajectory of the 5-DOF robot minimizes a cost function subject to constraints (see later).LetQj,it be the cubic polynomial for thejth joint defined on the interval Ii=ti,ti+1. Solving the interpolation problem means to find m-1 cubic polynomials satisfying the imposed conditions. In order to avoid considering cumbersome calculations with 4m-1 unknowns, it can be adopted that the second derivative of Qj,it is a linear function of t in the interval Ii; namely,(130)Q¨j,it=ti+1-thiQ¨j,iti+t-tihiQ¨j,iti+1t∈Ii,i=1,…,m-1,j=1,2,…,5.Integrating (130) and setting Qj,iti=qj,i and Qj,iti+1=qj,i+1, the following interpolating functions are obtained:(131)Q˙j,it=-ti+1-t2Q¨j,iti2hi+t-ti2Q¨j,iti+12hi+qj,i+1hi-hiQ¨j,iti+16-qj,ihi-hiQ¨j,iti6,t∈Ii,i=1,…,m-1,j=1,2,…,5,(132)Qj,it=ti+1-t3Q¨j,iti6hi+t-ti3Q¨j,iti+16hi+qj,i+1hi-hiQ¨j,iti+16t-ti+qj,ihi-hiQ¨j,iti6ti+1-t,t∈Ii,i=1,…,m-1,j=1,2,…,5.The joint displacements of these two extra via-points are given as(133)qj,2=qj,1+h1Q˙j,1t1+Q¨j,1t1h123+Q¨j,2t2h126,j=1,2,…,5,qj,m-1=qj,m-hm-1Q˙j,mtm-1+Q¨j,mtmhm-123+Q¨j,m-1tm-1hm-126,j=1,2,…,5.Since the derivative ofQj,it is continuous at t∈t0,tm, the inner via-points can be expressed as follows:(134)Q˙j,iti-0=Q˙j,iti+0i=1,2,…,m-1.CombiningQ˙j,iti=Q˙j,i-1tii=1,2,…,m-1 and (134) gives(135)μiAj,i-1+2Aj,i+λiAj,i+1=Bj,i,i=2,3,…,m-1,j=1,2,…,5,where(136)λi=hihi+hi-1,i=2,3,…,m-1,μi=1-λi,i=2,3,…,m-1,Bj,i=6qj,i+1-qj,i/hi-qj,i-qj,i-1/hi-1hi+hi-1,i=2,3,…,m-1,j=1,2,…,5.Then, a linear system withm-2 unknowns (the acceleration at the inner via-points) is obtained as(137)KAj=Bjj=1,…,5,where Aj and Bj are m-2-dimensional vector and the coefficient matrix K (the same for all joints) is a nonsingular and band-diagonal m-2×m-2 matrix as follows:(138)K=2λ1μ22λ2⋱⋱⋱μm-22λm-2μm-12.Given the acceleration and velocities for the first and the last via-point, this linear system withm-2 unknowns can be obtained. Thus, the acceleration at the inner via-points, namely,(139)Aj=aj,2⋮aj,m-1,j=1,…,5,with Q¨j,iti=aj,i,i=2,3,…,m-1, can be obtained by solving the linear system (137).Thus, the actual motion trajectory of the 5-DOF robot is constructed by joining together the above-mentioned spline functions; that is, ift∈Ii then qjt=Qj,it, q˙jt=Q˙j,it, q¨jt=Q¨j,it, q⃛jt=Q⃛j,it, Ukt=uk,it,k=1,2,…,5,j=1,2,…,5, andi=1,2,…,m-1 (which is a new symbol for force defined for t∈Ii).In order to solve the trajectory planning problem, considering that the trajectory is formed by cubic splines, an explicit expression for objective functions and constraints needs to be formulated.As for the displacement, the constraints are denoted by(140)QCmin⁡_j,i≤Qj,it≤QCmax⁡_j,i⁡t∈Ii,j=1,…,5,i=1,…,m-1.As for the velocity, the constraints are given by(141)Q˙j,it≤VCjt∈Ii,j=1,…,5,i=1,…,m-1.Here we note that these constraints are of the semi-infinite type; that is, they must hold for any value of the continuous variablet in the indicated time interval. It would be much easier to deal with them if they can be transformed into a finite form. This is feasible noticing that the velocity is parabolic in the interval Ii=ti,ti+1; hence it reaches its maximum value either at one of the interval ends or at a certain intermediate instant tj,i∗ when the acceleration becomes zero. This intermediate instant tj,i∗ can be expressed as(142)tj,i∗=ti-hiaj,iaj,i+1-aj,ij=1,…,5,i=1,…,m-1.The value of the velocity at the instant tj,i∗ is obtained as(143)Q˙j,itj,i∗=-hiaj,iaj,i+12aj,i+1-aj,i+qj,i+1-qj,ihi-hiaj,i+1-aj,i6,j=1,…,5,i=1,…,m-1.The values of the velocity at the instant ends are given by(144)Q˙j,iti=-hiaj,i2+qj,i+1-qj,ihi-hiaj,i+1-aj,i6,j=1,…,5,i=1,…,m-1,Q˙j,iti+1=hiaj,i+12+qj,i+1-qj,ihi-hiaj,i+1-aj,i6,j=1,…,5,i=1,…,m-1.Hence, the semi-infinite constraints (26) can be expressed much more accurately as(145)max⁡Q˙j,itj,i∗,Q˙j,iti,Q˙j,iti+1≤VCjj=1,…,5i=1,…,m-1which represent a finite number of relationships that must be satisfied for the planned trajectory.The kinematic constraints on acceleration and jerk can be similarly reduced to a finite form. We note that the acceleration is linear in the intervalIi=ti,ti+1 and therefore it must reach its maximum value at one of two interval ends. Hence, the semi-infinite constraints on the acceleration,(146)Q¨j,it≤WCjt∈Ii,j=1,…,5,i=1,…,m-1,can be expressed in a finite form as(147)max⁡aj,1,…,aj,m≤WCjj=1,…,5.The jerk can be obtained as follows:(148)Q⃛j,it=aj,i+1-aj,ihi,t∈Ii,j=1,…,5,i=1,…,m-1.As for the jerk, the semi-infinite constraint is given by(149)Q⃛j,it≤JCj,t∈Ii,j=1,…,5,i=1,…,m-1.Since the jerk is constant on any interval Ii=ti,ti+1, the finite form of the jerk yields(150)aj,i+1-aj,ihi≤JCjj=1,…,5,i=1,…,m-1.As for the force/torque, the semi-infinite constraint gives(151)uj,it≤UCj,t∈Ii,j=1,…,5,i=1,…,m-1;that is, F1,it≤UC1, F2,it≤UC2, F3,it≤UC3, τ4,it≤UC4, and τ5,it≤UC5, i=1,…,m-1.Moreover, in the optimization of the trajectory, the time intervalshihi>0,i=1,…,m-1 which are the true optimization parameters (i.e., the decision variables) must be particularly considered. Since in any interval the condition qj,i+1-qj,i/hi≤VCj must be satisfied. Hence, the lower bound of the intervals hi must be subjected to(152)hi>wi=maxj=1,…,5⁡qj,i+1-qj,iVCj>0,i=1,…,m-1.Thus, the finite dimensional multiobjective optimization problem is formulated as follows. Computex∈Rm-1 so as to minimize the cost function Fc:(153)Fc=w1z1ξ1+w2z2ξ2+w3z3ξ3+w4z4ξ4,where(154)z1=∑i=1m-1hi,z2=∑j=15∑i=1m-1hi2uj,iTtiQ˙j,iti2+uj,i+1Tti+1Q˙j,i+1ti+12,z3=∑j=15∑i=1m-2hi2aj,i+1-aj,ihi2+aj,i+2-aj,i+1hi+12,z4=∑j=15∑i=1m-1hi2aj,i2+aj,i+12,subject to the constraints (which are (140), (141), (147), (150)–(152), (29)–(32), (75), (79), (82), (86), and (87)).The vector of decision variablesx is defined by the sequence of the time intervals hi associated with each two consecutive via-points; thus x=h1,h2,…,hm-1T. When the intervals hi and joint displacements (i.e., these two extra via-points and joint displacements qj,i obtained by solving the inverse kinematics of the 5-DOF robot manipulator) are determined, other functions Qj,it, Q˙j,it, Q¨j,it, Q⃛j,it, and uj,it can also be determined by these time intervals and joint displacements. The initial and final velocity (q˙j,1 and q˙j,m) and acceleration (q¨j,1 and q¨j,m) for all joints j=1,2,…,5 are set to zero corresponding to the initial and final time instants. The payload mass mp is fixed to a feasible value satisfying (123).A genetic optimization algorithm NSGA-II is applied in order to try and solve the above-mentioned optimization problem. The algorithm NSGA-II runs for a number of iterations and then stops after meeting some termination criterion, thus producing a vector of decision variablesx∗ in solving trajectory planning and DLCC of the 5-DOF robot. These obtained decision variables x∗ and other above-mentioned functions can be represented as the best found solution that have been able to find so far.Actually, the real advantages of NSGA-II used in this application compared to the conventional constrained optimization algorithms (e.g., sequential quadratic programming) are that the former is population-based approach search performing the genetic operations like crossover and mutation for the elements of the decision variables (where the unfit individuals are replaced by the fit individuals through the crossover and mutation operator in population), thus discovering the global optimal solution is possible; also, the former is no need to provide any auxiliary relationships (e.g., gradients and Hessian matrix); moreover, the latter is easy to fall into local minima as the susceptibility to initial values, and it needs to calculate the gradients and the Hessian matrix of the objective functions and constraints resulting in more computational time. In Section6, a classical nonlinear constrained optimization function (i.e., the function fmincon in Matlab Optimization Toolbox) based on sequential quadratic programming algorithm is also applied to solve the above-mentioned optimization problem and compared with the best found solution of the genetic optimization algorithm NSGA-II.


## 3.1. Cost Function and Constraints

In this paper, the cost function is the weighted sum of four objective functions representing traveling time, total energy involved in the motion of the robot manipulator, joint jerks, and joint acceleration, respectively. The cost function is considered as the evaluation criterion for the optimal trajectory planning problem.In practice, the results of the multiobjective optimization problems are influenced by the values of the weighting coefficients. Hence, appropriate values are required to characterize the importance of the corresponding decision criteria for the trajectory planning applications. Multiple objectives can be combined into a scalar cost function via a weight vector. Weights may be assigned through the direct assignment, the eigenvector method, the empty method, the minimal information method, the random determination, or the adaptation determination. In this paper, it is considered that minimizing operation time (minimum traveling time) and saving energy (minimizing total energy involved in the motion) take the highest priority over others. The next priority is applied for reducing vibrations of robot manipulator joints (minimizing joint jerks and acceleration). Thus, a corresponding vector of normalized weights,w1=0.3, w2=0.3, w3=0.2 and w4=0.2, is assigned for the objective functions in the optimization problem.Significant differences may be made in the magnitude of each function when the objective functions are simply weighted and used to produce a single cost function, resulting in a dominating effort that is mainly caused by the function corresponding to the largest magnitude. In particular, the larger the magnitude of an objective function, the higher the influence on the cost function. Hence, a feasible and efficient means will be needed to deal with this problem effected by the large magnitude of the objective function. One possibility of avoiding this problem is to make an adequate normalization of the objective functions and formulate the same magnitude of objective functions. It can have a roughly equivalent influence on the objective functions for the cost function. Also normalizing parameters make all objective functions as unitless functions. In this paper, the normalized weighted objective functions method is used to balance the influence on four objective functions and select the best found solution. More details on this method can be referred to [47, 48]. The values of ξ1=1, ξ2=1000, ξ3=10, and ξ4=1 are taken as the normalizing parameters of the objective functions z1, z2, z3, and z4.As mentioned above, joint variablesq=[XS1XS2XS3φ4φ5]T of this robot manipulator form a vector of generalized coordinates. Likewise, the variables q˙, q¨, q⃛, and U=[F1F2F3τ4τ5]T are generalized velocities, acceleration, jerks, and forces.Given the following date, (a) the initial timet0t0≥0; the final time tf which is unknown and must be determined (see below); (b) the coordinates of the desired inertial position trajectory of the 5-DOF robot end-effector specified in two forms (see Section 2.1.3), the resulting desired initial and final conditions of the generalized coordinates qdt0 and qdtf are calculated by using (29)–(32); set qt0=qdt0 and qtf=qdtf; (c) the initial and final conditions are q˙t0, q¨t0, q⃛t0, q˙tf, q¨tf, and q⃛tf; thus, the basic multiobjective optimization problem is stated as follows.Compute the final timetf, the payload mass mp, and a smooth 5-DOF robot motion trajectory qt,q˙t,q¨t,q⃛tt∈[t0,tf] satisfying the above-mentioned initial and final conditions so as to minimize the cost function Fc given by(112)Fc=w1z1ξ1+w2z2ξ2+w3z3ξ3+w4z4ξ4,Where one has the following:Total traveling time between initial and final configurations:(113)z1=tf-t0.Total energy involved in the motion of the robot manipulator:(114)z2=∫t0tf∑j=15UjTtq˙jt2dt.Integral of joint jerks squared:(115)z3=∫t0tf∑j=15q⃛jt2dt.Integral of joint acceleration squared:(116)z4=∫t0tf∑j=15q¨jt2dt.subject to the following constraints:The final time constraint:(117)tf≥t0.The displacement constraint:(118)QCmin_j≤qjt≤QCmax_jj=1,…,5,t∈t0,tf.The velocity constraint:(119)q˙jt≤VCjj=1,…,5,t∈t0,tf.The acceleration constraint:(120)q¨jt≤WCjj=1,…,5,t∈t0,tf.The jerk constraint:(121)q⃛jt≤JCjj=1,…,5,t∈t0,tf.The force/torque constraint:(122)Ujt≤UCjj=1,…,5,t∈t0,tf;that is, F1t≤UC1,F2t≤UC2,F3t≤UC3,τ4t≤UC4,τ5t≤UC5,t∈t0,tf.The payload constraint:(123)mp_min≤mp≤mp_max.The kinematic constraints: that is, (29)–(32) (which are specific specified in Section 2.1).The dynamic constraints: that is, (75), (79), (82), and (86)–(88) (which are specific specified in Section 2.2 are also considered for the multiobjective optimization problem).Here,QCmin_j and QCmax_j are thejth joint lower and upper bounds of displacement, respectively. VCj, WCj, JCj, and UCj are thejth joint upper bounds of velocity, acceleration, jerk, and force/torque, respectively. mp_min and mp_max are the minimum and maximum allowable load corresponding to the DLCC of the robot manipulator, respectively.


## 3.2. Finite Dimensional 5-DOF Robot Motion Trajectory Representation

The cubic spline in trajectory planning is very useful, because the generated trajectories have continuous values of the acceleration; also, unlike higher-order polynomials, cubic splines would not generate problems such as excessive oscillations and overshoot between any pair of reference points.In this paper, cubic splines are applied to represent the trajectory of the joint space, given that the desired inertial position trajectory of the 5-DOF robot end-effector is specified in parametric form (see Section2.1.3). Also, given a positive integer m0, select an increasing sequence of m0 parameter values b1,b2,…,bm0 lying in the parameter interval b0,bf such that(124)b0=b1<b2<⋯<bm0=bf.Thus,m0 via-points points are obtained on the desired inertial position trajectory of the 5-DOF robot end-effector with coordinates(125)xdbi,ydbi,zdbii=1,2,…,m0.By using the inverse kinematics of the 5-DOF robot (see (29)–(32)), m0 via-points are obtained on the desired trajectory of the 5-DOF robot generalized coordinates qdt at a sequence of m0 increasing time instants indexed as follows:(126)t0=t1<t3<⋯<tm-2<tm,where the initial time t0 is given and m=m0+2.An additional two via-pointsqdt2 and qdtm-1, corresponding to times t2 and tm-1, are added in order to accommodate the specified initial and final conditions q˙t1, q¨t1, q˙tm, and q¨tm (see below).Thus, a total ofm via-points on the desired trajectory of the 5-DOF robot generalized coordinates qdt are known and are denoted as follows:(127)qj,i=qdjtij=1,2,…,5,i=1,2,…,m.What is not known and must be determined is the sequence ofm-1 increasing time instants,(128)t0=t1<t2<t3<⋯<tm-2<tm-1<tm.The sequence of time instantsti,i=1,2,…,m, defines m-1 consecutive time intervals Ii given by(129)Ii=ti,ti+1,i=1,2,…,m-1.Let hi denote the length of the time interval Ii; that is, hi=ti+1-ti,i=1,2,…,m-1.Given the above-mentionedm via-points and the time-interval lengths hi,i=1,2,…,m-1, then the actual motion trajectory of the 5-DOF robot is constructed by using a cubic spline segment to connect every pair of consecutive via-points; see (127).In this section, a methodology is proposed for computing the unknown time-interval lengthshi,i=1,2,…,m-1, such that the resulting actual motion trajectory of the 5-DOF robot minimizes a cost function subject to constraints (see later).LetQj,it be the cubic polynomial for thejth joint defined on the interval Ii=ti,ti+1. Solving the interpolation problem means to find m-1 cubic polynomials satisfying the imposed conditions. In order to avoid considering cumbersome calculations with 4m-1 unknowns, it can be adopted that the second derivative of Qj,it is a linear function of t in the interval Ii; namely,(130)Q¨j,it=ti+1-thiQ¨j,iti+t-tihiQ¨j,iti+1t∈Ii,i=1,…,m-1,j=1,2,…,5.Integrating (130) and setting Qj,iti=qj,i and Qj,iti+1=qj,i+1, the following interpolating functions are obtained:(131)Q˙j,it=-ti+1-t2Q¨j,iti2hi+t-ti2Q¨j,iti+12hi+qj,i+1hi-hiQ¨j,iti+16-qj,ihi-hiQ¨j,iti6,t∈Ii,i=1,…,m-1,j=1,2,…,5,(132)Qj,it=ti+1-t3Q¨j,iti6hi+t-ti3Q¨j,iti+16hi+qj,i+1hi-hiQ¨j,iti+16t-ti+qj,ihi-hiQ¨j,iti6ti+1-t,t∈Ii,i=1,…,m-1,j=1,2,…,5.The joint displacements of these two extra via-points are given as(133)qj,2=qj,1+h1Q˙j,1t1+Q¨j,1t1h123+Q¨j,2t2h126,j=1,2,…,5,qj,m-1=qj,m-hm-1Q˙j,mtm-1+Q¨j,mtmhm-123+Q¨j,m-1tm-1hm-126,j=1,2,…,5.Since the derivative ofQj,it is continuous at t∈t0,tm, the inner via-points can be expressed as follows:(134)Q˙j,iti-0=Q˙j,iti+0i=1,2,…,m-1.CombiningQ˙j,iti=Q˙j,i-1tii=1,2,…,m-1 and (134) gives(135)μiAj,i-1+2Aj,i+λiAj,i+1=Bj,i,i=2,3,…,m-1,j=1,2,…,5,where(136)λi=hihi+hi-1,i=2,3,…,m-1,μi=1-λi,i=2,3,…,m-1,Bj,i=6qj,i+1-qj,i/hi-qj,i-qj,i-1/hi-1hi+hi-1,i=2,3,…,m-1,j=1,2,…,5.Then, a linear system withm-2 unknowns (the acceleration at the inner via-points) is obtained as(137)KAj=Bjj=1,…,5,where Aj and Bj are m-2-dimensional vector and the coefficient matrix K (the same for all joints) is a nonsingular and band-diagonal m-2×m-2 matrix as follows:(138)K=2λ1μ22λ2⋱⋱⋱μm-22λm-2μm-12.Given the acceleration and velocities for the first and the last via-point, this linear system withm-2 unknowns can be obtained. Thus, the acceleration at the inner via-points, namely,(139)Aj=aj,2⋮aj,m-1,j=1,…,5,with Q¨j,iti=aj,i,i=2,3,…,m-1, can be obtained by solving the linear system (137).Thus, the actual motion trajectory of the 5-DOF robot is constructed by joining together the above-mentioned spline functions; that is, ift∈Ii then qjt=Qj,it, q˙jt=Q˙j,it, q¨jt=Q¨j,it, q⃛jt=Q⃛j,it, Ukt=uk,it,k=1,2,…,5,j=1,2,…,5, andi=1,2,…,m-1 (which is a new symbol for force defined for t∈Ii).In order to solve the trajectory planning problem, considering that the trajectory is formed by cubic splines, an explicit expression for objective functions and constraints needs to be formulated.As for the displacement, the constraints are denoted by(140)QCmin⁡_j,i≤Qj,it≤QCmax⁡_j,i⁡t∈Ii,j=1,…,5,i=1,…,m-1.As for the velocity, the constraints are given by(141)Q˙j,it≤VCjt∈Ii,j=1,…,5,i=1,…,m-1.Here we note that these constraints are of the semi-infinite type; that is, they must hold for any value of the continuous variablet in the indicated time interval. It would be much easier to deal with them if they can be transformed into a finite form. This is feasible noticing that the velocity is parabolic in the interval Ii=ti,ti+1; hence it reaches its maximum value either at one of the interval ends or at a certain intermediate instant tj,i∗ when the acceleration becomes zero. This intermediate instant tj,i∗ can be expressed as(142)tj,i∗=ti-hiaj,iaj,i+1-aj,ij=1,…,5,i=1,…,m-1.The value of the velocity at the instant tj,i∗ is obtained as(143)Q˙j,itj,i∗=-hiaj,iaj,i+12aj,i+1-aj,i+qj,i+1-qj,ihi-hiaj,i+1-aj,i6,j=1,…,5,i=1,…,m-1.The values of the velocity at the instant ends are given by(144)Q˙j,iti=-hiaj,i2+qj,i+1-qj,ihi-hiaj,i+1-aj,i6,j=1,…,5,i=1,…,m-1,Q˙j,iti+1=hiaj,i+12+qj,i+1-qj,ihi-hiaj,i+1-aj,i6,j=1,…,5,i=1,…,m-1.Hence, the semi-infinite constraints (26) can be expressed much more accurately as(145)max⁡Q˙j,itj,i∗,Q˙j,iti,Q˙j,iti+1≤VCjj=1,…,5i=1,…,m-1which represent a finite number of relationships that must be satisfied for the planned trajectory.The kinematic constraints on acceleration and jerk can be similarly reduced to a finite form. We note that the acceleration is linear in the intervalIi=ti,ti+1 and therefore it must reach its maximum value at one of two interval ends. Hence, the semi-infinite constraints on the acceleration,(146)Q¨j,it≤WCjt∈Ii,j=1,…,5,i=1,…,m-1,can be expressed in a finite form as(147)max⁡aj,1,…,aj,m≤WCjj=1,…,5.The jerk can be obtained as follows:(148)Q⃛j,it=aj,i+1-aj,ihi,t∈Ii,j=1,…,5,i=1,…,m-1.As for the jerk, the semi-infinite constraint is given by(149)Q⃛j,it≤JCj,t∈Ii,j=1,…,5,i=1,…,m-1.Since the jerk is constant on any interval Ii=ti,ti+1, the finite form of the jerk yields(150)aj,i+1-aj,ihi≤JCjj=1,…,5,i=1,…,m-1.As for the force/torque, the semi-infinite constraint gives(151)uj,it≤UCj,t∈Ii,j=1,…,5,i=1,…,m-1;that is, F1,it≤UC1, F2,it≤UC2, F3,it≤UC3, τ4,it≤UC4, and τ5,it≤UC5, i=1,…,m-1.Moreover, in the optimization of the trajectory, the time intervalshihi>0,i=1,…,m-1 which are the true optimization parameters (i.e., the decision variables) must be particularly considered. Since in any interval the condition qj,i+1-qj,i/hi≤VCj must be satisfied. Hence, the lower bound of the intervals hi must be subjected to(152)hi>wi=maxj=1,…,5⁡qj,i+1-qj,iVCj>0,i=1,…,m-1.Thus, the finite dimensional multiobjective optimization problem is formulated as follows. Computex∈Rm-1 so as to minimize the cost function Fc:(153)Fc=w1z1ξ1+w2z2ξ2+w3z3ξ3+w4z4ξ4,where(154)z1=∑i=1m-1hi,z2=∑j=15∑i=1m-1hi2uj,iTtiQ˙j,iti2+uj,i+1Tti+1Q˙j,i+1ti+12,z3=∑j=15∑i=1m-2hi2aj,i+1-aj,ihi2+aj,i+2-aj,i+1hi+12,z4=∑j=15∑i=1m-1hi2aj,i2+aj,i+12,subject to the constraints (which are (140), (141), (147), (150)–(152), (29)–(32), (75), (79), (82), (86), and (87)).The vector of decision variablesx is defined by the sequence of the time intervals hi associated with each two consecutive via-points; thus x=h1,h2,…,hm-1T. When the intervals hi and joint displacements (i.e., these two extra via-points and joint displacements qj,i obtained by solving the inverse kinematics of the 5-DOF robot manipulator) are determined, other functions Qj,it, Q˙j,it, Q¨j,it, Q⃛j,it, and uj,it can also be determined by these time intervals and joint displacements. The initial and final velocity (q˙j,1 and q˙j,m) and acceleration (q¨j,1 and q¨j,m) for all joints j=1,2,…,5 are set to zero corresponding to the initial and final time instants. The payload mass mp is fixed to a feasible value satisfying (123).A genetic optimization algorithm NSGA-II is applied in order to try and solve the above-mentioned optimization problem. The algorithm NSGA-II runs for a number of iterations and then stops after meeting some termination criterion, thus producing a vector of decision variablesx∗ in solving trajectory planning and DLCC of the 5-DOF robot. These obtained decision variables x∗ and other above-mentioned functions can be represented as the best found solution that have been able to find so far.Actually, the real advantages of NSGA-II used in this application compared to the conventional constrained optimization algorithms (e.g., sequential quadratic programming) are that the former is population-based approach search performing the genetic operations like crossover and mutation for the elements of the decision variables (where the unfit individuals are replaced by the fit individuals through the crossover and mutation operator in population), thus discovering the global optimal solution is possible; also, the former is no need to provide any auxiliary relationships (e.g., gradients and Hessian matrix); moreover, the latter is easy to fall into local minima as the susceptibility to initial values, and it needs to calculate the gradients and the Hessian matrix of the objective functions and constraints resulting in more computational time. In Section6, a classical nonlinear constrained optimization function (i.e., the function fmincon in Matlab Optimization Toolbox) based on sequential quadratic programming algorithm is also applied to solve the above-mentioned optimization problem and compared with the best found solution of the genetic optimization algorithm NSGA-II.


## 4. The Proposed Approach for Trajectory Planning

In this section, an approach is proposed to deal with the multiobjective optimization problem for a given prespecified trajectory taking into account a feasible DLCC and obtain time intervals corresponding to the via-points, joint displacement, velocity, acceleration, jerk, and force/torque, considering the objective functions and two categories of constraints (inequality linear constraints and equality nonlinear constraints). This feasible DLCC is marked asDLCCfe and must fall within the DLCC range of reachable values. The detailed calculation for maximum DLCC will be presented in the next section.Note that the cost function results would converge after a certain number of generations by using the calculation mechanism of the NSGA-II algorithm. In this paper, this stabilized condition is made as the termination criterion, and the corresponding calculation results are taken as the best found solution for the trajectory planning problem and maximum DLCC calculation of this 5-DOF robot manipulator.The desired joint displacement (i.e., the via-points in the joint space) can be determined by solving the robot manipulator inverse kinematics problem. When the optimization process is performed, the equality nonlinear constraints and inequality linear constraints described in the previous section are considered in the objective functions. The suitable population size (pop), total number of generations (gen), crossover probability (pc), mutation probability (pm), crossover distribution index (dc), and mutation distribution index (dm) in NSGA-II algorithm are needed to select so that the algorithm leads the robot manipulator to have a smooth trajectory and minimum chattering in profiles of velocity, acceleration, jerk, and force/torque.The detailed description of the proposed approach for trajectory planning is presented in the following steps.Step 1.
Calculate desired joint displacement (i.e., the via-points in the joint space) according to the given desired trajectory in the task space by solving the robot manipulator inverse kinematics problem using (29)–(32).Step 2.
SelectDLCCfe, pop, gen, pc, pm, dc, and dm, set the initial and final velocity (q˙j,1 and q˙j,m) and acceleration (q¨j,1 and q¨j,m) for all joints j=1,2,…,5, set the constraints of displacement, velocity, acceleration, jerk, and force/torque, and calculate the lower bound of the time intervals using (152).Step 3.
A set of small time-interval lengthshi0,i=1,…,m-3 (except the time intervals of two extra via-points), are randomly generated using (152) and the constraints QCmin, QCmax, VC, WC, JC, and UC need to be satisfied, then, respectively, interpolating randomly at the first and last time-interval lengths (i.e., h10 and hm-30) for the time intervals of two extra via-points generates new time-interval lengths h1_10, h1_20, hm-3_10, and hm-3_20, and finally assembling h1_10, h1_20, h20, h30,…,hm-40, hm-3_10, and hm-3_20 constructs the initialized time-interval lengths hi,i=1,…,m-1; generate two extra via-points using (133), and arrange the displacement Q in the NSGA-II algorithm.Step 4.
If lower bound constraints of the time intervals are satisfied (which are judged by (152)), go back to Step 5. Otherwise, go back to Step 3.Step 5.
CalculateQ˙, Q¨, Q⃛, and U using (131), (137), and (148) and the dynamic equations (75), (79), (82), (86), and (87), respectively. If the constraints (displacement, velocity, acceleration, jerk, and force/torque) are satisfied (which are judged by (140), (145), (147), (150), and (151), resp.), go back to Step 6. Otherwise, go back to Step 3.Step 6.
Optimize the time intervals (i.e., the decision variables) and objective functions (which are formulated as (154)) by using the calculation mechanism of the NSGA-II algorithm, compare the corresponding cost function result (i.e., (153)) of each population for the current generation, and find out the minimum one (Fci=min⁡Fci,1,…,Fci,p, wherei denotes the current ith generation and p is the number of the total population) and the corresponding best found calculation results (decision variables, i.e., the time intervals, and the corresponding results of objective functions, displacement, velocity, acceleration, jerk, and force/torque). Then, save and report these selected best found calculation results for each generation. This step continues until all generations are completed.Step 7.
Compare the cost function results of each generation, and find out the minimum one (Fc=min⁡Fc1,…,Fcg, where g is the total number of generations) and the corresponding other calculation results for the whole calculation process.Through this detailed calculation process, the required calculation decision variables, that is, the time intervals, and the corresponding best found calculation values (objective functions, cost function, displacement, velocity, acceleration, jerk, and force/torque), taking equality nonlinear constraints and inequality linear constraints into account, are obtained by using the NSGA-II algorithm.


## 5. The Proposed Approach for DLCC Calculation

In many practical applications, the maximum DLCC must be a feasible one to satisfy certain performance requirements of the robot manipulator. It is appropriate to specify a robot manipulator in terms of useful payload as a function of performance rather than just in terms of maximum capacity. Therefore, a feasible maximum DLCC of the robot manipulator, from a safety and reliability point of view, plays a significant role in practical applications. The dominant constraint that limits the DLCC of a robot manipulator is the limitations on the applied forces/torques in the robot manipulator joints. A feasible maximum DLCC, considering this dominant constraint (i.e., the applied forces/torques), is addressed in this section.Another approach is proposed to calculate the maximum DLCC for a given prespecified desired trajectory which is described in the task space considering the objective functions and two categories of constraints as mentioned above. Firstly, a feasible and suitable DLCC is introduced for the trajectory planning based on the proposed approach in the above section by solving the multiobjective optimization problem for this given trajectory, and the related best found results (its best found time intervals, joint displacement, velocity, acceleration, and jerk) can be obtained. Then these determined results are taken as the initial conditions to calculate the maximum DLCC and also the motor torque requirements using the iterative method. The process of the iterative solution is repeated until the termination condition (i.e., the motor torque requirements are beyond the rated torques) is reached. Here, at each iteration, the DLCC, that is, the allowable payload massmp that is carried by the robot manipulator, is increased by an iteration procedure. mp_in and Δmp are the initial value and the increment of mp at each iteration, respectively.The proposed approach for the maximum DLCC calculation proceeds in nine steps as follows.Step 1.
Calculate desired joint displacement (i.e., the via-points in the joint space) according to the given trajectory in the task space by solving the robot manipulator inverse kinematics problem using (29)–(32).Step 2.
Selectmp_in, Δmp, pop, gen, pc, pm, dc, and dm, set mp=mp_in, set the initial and final velocity (q˙j,1 and q˙j,m) and acceleration (q¨j,1 and q¨j,m) for all joints j=1,2,…,5, set the constraints of displacement, velocity, acceleration, jerk, and force/torque, and calculate the lower bound of the time intervals using (152).Step 3.
A set of small time-interval lengthshi0,i=1,…,m-3 (except the time intervals of two extra via-points), are randomly generated using (152) and the constraints QCmin, QCmax, VC, WC, JC, and UC need to be satisfied, then, respectively, interpolating randomly at the first and last time-interval lengths (i.e., h10 and hm-30) for the time intervals of two extra via-points generates new time-interval lengths h1_10, h1_20, hm-3_10, and hm-3_20, and finally assembling h1_10, h1_20, h20, h30,…,hm-40, hm-3_10, and hm-3_20 constructs the initialized time-interval lengths hi,i=1,…,m-1; generate two extra via-points using (133), and arrange the displacement Q in the NSGA-II algorithm.Step 4.
If lower bound constraints of the time intervals are satisfied (which are judged by (152)), go back to Step 5. Otherwise, go back to Step 3.Step 5.
CalculateQ˙, Q¨, Q⃛, and U using (131), (137), (148) and the dynamic equations (75), (79), (82), (86), and (87), respectively. If the constraints (displacement, velocity, acceleration, jerk, and force/torque) are satisfied (which are judged by (140), (145), (147), (150) and (151), resp.), go back to Step 6. Otherwise, go back to Step 3.Step 6.
Optimize the time intervals (i.e., the decision variables) and objective functions (which are formulated as (154)) by using the calculation mechanism of the NSGA-II algorithm, compare the cost function results of each generation, and find out the minimum one (Fc=min⁡Fc1,…,Fcg, where g is the total number of generations) and the corresponding other calculation results (decision variables, i.e., the time intervals, and the corresponding results of objective functions, displacement, velocity, acceleration, jerk, and force/torque). Then, save and report these selected best found results.Step 7.
SetDLCC=mp; increase payload mp by mp=mp+Δmp.Step 8.
Calculate the required motor torques using the dynamic equations (75), (79), (82), (86), and (87) according to the selected best found results (i.e., the best found displacement Q, velocity Q˙, and acceleration Q¨) and the current payload mass mp. If the constraints of the rated motor torques are satisfied (which are judged by (151)), go back to Step 7. Otherwise, go back to Step 9.Step 9.
Save and report the latest DLCC (i.e., the maximum DLCC).Through this detailed calculation steps while taking into account equality nonlinear constraints and inequality linear constraints, according to the best found initial conditions (i.e., the selected best found results of displacementQ, velocity Q˙, and acceleration Q¨), the maximum DLCC for the given trajectory are determined based on the NSGA-II algorithm and the iterative method.


## 6. Numerical Example

Based on the above proposed approaches, the trajectory planning and the maximum DLCC calculation of this robot manipulator for a given desired end-effector inertial position trajectory are presented in this section.A saddle line segment is selected as the given desired inertial position trajectory of the 5-DOF robot manipulator end-effector for the trajectory planning and the maximum DLCC calculation. The saddle line segment is defined between initial coordinates (xd=600mm, yd=-150mm, and zd=1200mm) and final coordinates (xd=1000mm, yd=150mm, and zd=950mm). In this paper, the equation of this saddle line segment is expressed in implicit form as follows:(155)zd=fxd,yd=yd-1002202-xd2802+1100.The saddle line segment is also expressed in parametric form as follows:(156)xd=600+40ηyd=-150+30ηzd=yd-1002202-xd2802+1100,where the parameter η∈[0,10]. In this case m0=11 via-points are chosen on the desired end-effector trajectory leading to the following values for the parameter η: η1=0,η2=1,…,η11=10.This saddle line segment (i.e., the prespecified desired trajectory) and each corresponding via-point and orientation in the task space are shown in Figure6, where the orientation is determined as follows: the vector a=ax,ay,azT is defined along the normal vector of the via-point (cyan arrows as shown in Figure 6), o=ox,oy,ozT is defined along the tangent vector of the via-pointand points the next via-point (blue arrows as shown in Figure 6), and n=nx,ny,nzT is defined by n=o×a. The generalized coordinates of this 5-DOF robot manipulator can be represented as q=XS1,XS2,XS3,φ4,φ5T. The robot manipulator is at rest initially and comes to a full stop at the end of the trajectory (i.e., q˙j,1=0, q˙j,m=0, q¨j,1=0, and q¨j,m=0 for all joints j=1,2,…,5). In this paper, joint force/torque of this robot manipulator is generated by AC servomotors. The feasible DLCC (i.e., DLCCfe) is taken as a fixed payload mass mp=10kg for solving the trajectory planning problem.Figure 6
The desired end-effector inertial position trajectory.The via-points (i.e., the desired joint displacement) including two extra via-points in the joint space are determined by solving the inverse kinematics of the robot manipulator and given in Table6. The real variable type is considered in the NSGA-II algorithm. The values of the parameter which is used in algorithm, population size (pop), total number of generations (gen), crossover probability (pc), mutation probability (pm), crossover distribution index (dc), and mutation distribution index (dm), are 100, 200, 0.9, 0.1, 10, and 100, respectively.Table 6
The desired joint displacement and specified points on desired end-effector inertial position trajectory.
Via-point indexi Time instantsti Joint 1 (mm) Joint 2 (mm) Joint 3 (mm) Joint 4 (rad) Joint 5 (rad) x d (ti) (mm) y d (ti) (mm) z d (ti) (mm) X S 1 , i X S 2 , i X S 3 , i q 4 , i q 5 , i 1 t 1 262.63 142.92 367.10 −1.030 1.028 600 −150 1200 2 t 2 264.82 149.16 361.35 −1.0374 1.0253 605.375 −146.070 1194.164 3 t 3 279.40 189.61 325.12 −1.082 0.993 640 −120 1157 4 t 4 298.23 236.26 288.21 −1.117 0.918 680 −90 1118 5 t 5 319.14 284.04 255.31 −1.149 0.838 720 −60 1083 6 t 6 341.88 333.02 223.38 −1.179 0.752 760 −30 1052 7 t 7 366.21 383.29 201.39 −1.206 0.662 800 0 1025 8 t 8 391.95 434.95 180.26 −1.232 0.566 840 30 1002 9 t 9 418.93 488.14 162.95 −1.257 0.466 880 60 983 10 t 10 447.03 543.04 149.39 −1.280 0.361 920 90 968 11 t 11 476.11 599.93 139.50 −1.303 0.252 960 120 957 12 t 12 502.37 651.71 134.09 −1.3412 0.2045 993.187 144.873 9.51221 13 t 13 507.92 662.77 132.92 −1.350 0.197 1000 150 950The proposed approaches are applied both to trajectory planning problem and maximum DLCC calculation for this 5-DOF robot manipulator, and the related best found solutions are obtained. Note that, without loss of generality, only the case of the rigid robot manipulator is stressed here; other contributions to the dynamic description of the robot manipulator (which may include the dynamics of the actuators, joint and link flexibility, friction, noise, and disturbances) are not considered in this paper. The relationship between the cost function and the total number of generations is shown in Figure7. It can be found that the cost function value reached the convergence to a stabilized condition from 66th to 200th generation. The relationships between objective functions (total traveling time (z1), total energy involved in the motion (z2), integral of joint jerks squared (z3), and integral of joint acceleration squared (z4)) and the corresponding total number of generations (pop) are shown in Figures 8(a), 8(b), 8(c), and 8(d), respectively.Figure 7
The relationship between cost function and generation.Figure 8
The relationship between objective functions and generation: (a) total traveling time; (b) total energy; (c) integral of jerks squared; and (d) integral of acceleration squared.
(a) (b) (c) (d)The best found variables (i.e., time intervals) obtained from the proposed approach for trajectory planning are given in Table7. The best found joint displacement is given in Table 8. The best found joint displacement, velocity, acceleration, jerk, and force/torque are shown in Figures 9, 10, 11, 12, and 13, respectively. The actual end-effector inertial position is obtained as shown in Figure 14. The end-effector inertial position tracking error between the actual inertial motion trajectory and the desired inertial position trajectory is shown in Figure 15. The standard deviation tracking error, the variance tracking error, and the maximum tracking error are 0.0840 mm, 0.0071 mm, and 0.2933 mm (the last one shows the nearby point of the first extra point), respectively.Table 7
The best found variables (time interval lengths).
h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 h 10 h 11 h 12 0.330 0.837 0.803 0.569 0.477 0.382 0.348 0.348 0.392 0.552 0.732 0.420Table 8
The best found joint displacement and actual points on end-effector inertial position trajectory.
Via-point indexi Time instantsti (sec) Joint 1 (mm) Joint 2 (mm) Joint 3 (mm) Joint 4 (rad) Joint 5 (rad) x ( t i ) (mm) y ( t i ) (mm) z ( t i ) (mm) X S 1 , i X S 2 , i X S 3 , i q 4 ( t i ) q 5 ( t i ) 1 0 262.63 142.92 367.10 −1.030 1.028 599.974 −149.996 1199.998 2 0.330 263.57 145.35 364.92 −1.033 1.027 602.064 −148.530 1197.757 3 1.167 279.40 189.61 325.12 −1.082 0.993 639.988 −120.009 1156.971 4 1.970 298.23 236.26 288.21 −1.117 0.918 680.018 −89.937 1117.988 5 2.530 319.14 284.04 255.31 −1.149 0.838 719.961 −59.994 1082.990 6 3.016 341.88 333.02 223.38 −1.179 0.752 759.998 −30.042 1052.018 7 3.397 366.21 383.29 201.39 −1.206 0.662 799.999 0.022 1025.016 8 3.745 391.95 434.95 180.26 −1.232 0.566 839.951 30.000 1002.038 9 4.093 418.93 488.14 162.95 −1.257 0.466 879.967 59.997 983.048 10 4.485 447.03 543.04 149.39 −1.280 0.361 919.970 89.985 968.014 11 5.037 476.11 599.93 139.50 −1.303 0.252 959.982 119.989 956.999 12 5.769 505.32 667.51 133.42 −1.346 0.1969 996.812 147.566 950.498 13 6.189 507.92 662.77 132.92 −1.350 0.197 999.993 150.042 949.980Figure 9
The best found joint displacement: (a) joints 1, 2, and 3 and (b) joints 4 and 5.
(a) (b)Figure 10
The best found joint velocity: (a) joints 1, 2, and 3 and (b) joints 4 and 5.
(a) (b)Figure 11
The best found joint acceleration: (a) joints 1, 2, and 3 and (b) joints 4 and 5.
(a) (b)Figure 12
The best found joint jerk: (a) joints 1, 2, and 3 and (b) joints 4 and 5.
(a) (b)Figure 13
The best found joint force/torque: (a) joints 1, 2, and 3 and (b) joints 4 and 5.
(a) (b)Figure 14
Actual end-effector inertial position.Figure 15
End-effector inertial position tracking error.The best found joint displacement, velocity, and acceleration are taken as the initial conditions (the same as shown in Figures9, 10, and 11, resp.) to calculate the maximum DLCC and also the motor torque requirements (i.e., requirements of joint force/torque). Note that, without loss of generality, only the case of the rigid robot manipulator is stressed here; other contributions to the dynamic description of the robot manipulator (which may include the dynamics of the actuators, joint and link flexibility, friction, noise, and disturbances) are not considered in this paper. The proposed DLCC calculation approach is applied and the maximum DLCC, that is, the maximum allowable load, is found to be 12.65 kg.In this paper, the cost function value converges to a stabilized status from 66th to 200th generation, so produced vector of decision variablesx∗ (i.e., the time-interval lengths hi,i=1,2,…,12) and other above-mentioned functions of the last generation are used to represent the solution of trajectory planning and DLCC of the 5-DOF robot. Strictly speaking, it cannot be rigorously proved that this found solution x∗ is the constrained global minimizer of the cost function Fc and it only represents the best found solution that have been able to find so far.The Matlab function of the proposed approach for trajectory planning implementing the optimization algorithm NSGA-II and an example multimodal cost function subject to some nonlinear constraints showing how to call/use the function implementing the NSGA-II algorithm are given in AppendixA and Appendix B (both in Supplementary Materials available online at http://dx.doi.org/10.1155/2016/1302537), respectively.A classical nonlinear programming solver, that is, fmincon in Matlab Optimization Toolbox (which is based on sequential quadratic programming algorithm), is applied to solve the above-mentioned minimum of constrained nonlinear objective functions optimization problem and compared with the best found solution of the genetic optimization algorithm NSGA-II.Initializing randomly the time-interval lengthshi,i=1,…,m-1, just like the initialized way of Step 3 in Section 4 generates the initial value of the fmincon function. The same normalized weights (wii=1,2,3,4) and normalizing parameters (ξii=1,2,3,4) as mentioned in Section 3 are used in fmincon solver. The same processes of initializing way and solving are performed 100 times and generate 100 solving results with 1032.78 seconds. The mean value of calculation results is listed in Table 9. The best found solution of the algorithm NSGA-II for the above-mentioned optimization problem (which is computed with is 74.55 seconds) is also listed in Table 9. The results of the cost function and the objective functions in fmincon function solver are given in Appendix C in Supplementary Materials.Table 9
Comparison for the solution of NSGA-II and fmincon solver.
Type Cost function (Fc) Total traveling time (z1) Total energy (z2) Integral of joint jerks squared (z3) Integral of joint acceleration squared (z4) Best found solution of NSGA-II 3.093 6.189 3921.602 1.068 0.194 Mean value solution of fmincon solver 3.220 7.146 3250.239 2.510 0.252Comparing these results, it can be observed that the found solution of the genetic optimization algorithm NSGA-II is better than the fmincon function solver with respect to the cost function (Fc), total traveling time (z1), integral of joint jerks squared (z3), and integral of joint acceleration squared (z4). This is because the former is population-based approach which can perform the genetic crossover and mutation operations for the elements of the decision variables and desert the unfit individuals in each generation. Also, these genetic operations can extend and discover the possibly global solution. The computational time of the former is less than the latter, mainly because the algorithm NSGA-II does not need to provide any auxiliary relationships such as the gradients and Hessian matrix of the objective functions and constraints.


## 7. Conclusions

A new methodology using a direct method is presented for obtaining the best found trajectory planning and maximum dynamic load-carrying capacity (DLCC) for a given prespecified trajectory based on an evolutionary algorithm (i.e., NSGA-II). A cubic spline curve is used to represent the trajectory. This problem is transcribed into a nonlinear constrained multiobjective optimization problem. To summarize, the major contributions of this paper are as follows:(i)
The nonlinear constrained multiobjective optimization problem is formulated with four objective functions (i.e., total traveling time, total energy involved in the motion, joint jerks, and joint acceleration) and a cost function (which is a weighted sum of these objective functions). These decision criteria can ensure the best found trajectory is smooth and lead to minimum actuator power and energy.(ii)
Two separate approaches are proposed to deal with trajectory planning problem and maximum DLCC calculation for a 5-DOF robot manipulator, respectively. These approaches are easy to program and implement and can also be extended to handle similar problems for other types of the robots.(iii)
A numerical example verified the effectiveness of the two proposed approaches and obtained best found solutions for trajectory planning and maximum DLCC calculation of a 5-DOF robot manipulator.In order to get more accurate and smoother trajectory, the nonuniform rational B-spline (NURBS) curve with more via-points will be used to represent the trajectory in future work. The presented new methodology based on the evolutionary optimization technique and two proposed approaches are expected to provide some insight into the foundational aspects of trajectory planning and maximum DLCC calculation.



---

*Source: 1302537-2016-12-25.xml*