Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots

Wang Shanda; Luo Xiao; Luo Qingsheng; Han Baoling

doi:10.3390/app9204365

Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots

Shanda, Wang;Xiao, Luo;Qingsheng, Luo;Baoling, Han 2019-10-16 00:00:00 applied sciences Article Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots 1 2 , 1 3 Wang Shanda , Luo Xiao *, Luo Qingsheng and Han Baoling School of Mechatronic Engineering, Beijing Institute of Technology, Beijing 100081, China; 3120160137@bit.edu.cn (S.W.); luoqsh@bit.edu.cn (Q.L.) School of Computer Science & Technology, Beijing Institute of Technology, Beijing 100081, China School of Mechanical Engineering, Beijing Institute of Technology, Beijing 100081, China; hanbl@bit.edu.cn * Correspondence: luox@bit.edu.cn; Tel.: +86-010-6891-8856 Received: 14 August 2019; Accepted: 10 October 2019; Published: 16 October 2019 Featured Application: This paper primarily studies the existence conditions for the closed-form inverse kinematic solution for revolute serial robots. Based on the results, a general closed-form inverse solution algorithm is designed. Abstract: This study proposes a method for judging the existence of closed-form inverse kinematics solutions based on the Denavit–Hartenberg (DH) model. In this method, serial robots with closed-form solutions are described using three types of sub-problems from the viewpoint of solving algebraic equations. If a serial robot can be described using these three types of sub-problems, i.e., if the inverse kinematics problems can be solved by several basic problems, then there is a closed-form solution. Based on the above method, we design a set of universal closed-form inverse kinematics solving algorithms. Since there is a deﬁnite formula solution for the three types of sub-problems, the joint angles can be rapidly determined. In addition, because the DH parameters can directly reﬂect the linkage of the robot, the judgment of the sub-problems is also quick and accurate. More importantly, the algorithm can be applied to serial robots with low degrees of freedom. This enables the algorithm to not only quickly and accurately solve inverse kinematics problems but also to exhibit high universality. This proposed theory improves the existence conditions for closed-form reverse solutions and further promotes the development of motion control techniques for serial robots. Keywords: inverse kinematics; Denavit–Hartenberg method; closed-form solution; existence condition; universal inverse kinematics algorithm 1. Introduction Recently, the use of serial robots has increased owing to their extensive application in the field of bionics and in various industries [1–5]. Overcoming the usual problems in inverse kinematics is a key to controlling the motion of serial robots. However, inverse kinematics is a non-linear problem with multiple solutions. Among these solutions, the general numerical solution method is both time-consuming and unstable. Alternatively, the closed-form solution for inverse kinematics is commonly sought for practical applications but has two major limitations that remain unsolved. First, there is no general method for finding the closed-form solution. Second, the Pieper criterion is not complete. To resolve these problems, a model for the robotic kinematics needs to be established. There are two major methods for robotic modeling using inverse kinematics, namely the Denavit–Hartenberg (DH) parametric method and the product of exponentials (POE) formula method. Appl. Sci. 2019, 9, 4365; doi:10.3390/app9204365 www.mdpi.com/journal/applsci Appl. Sci. 2019, 9, 4365 2 of 36 The DH parametric modeling method is based on a transformation of coordinates [6]. Due to its easily visualized parameters, this convenient method has attracted the attention of many researchers and engineers. Under this model, the inverse kinematics problem is solved via two methods, i.e., numerical and closed-form solutions. Moreover, because the forward kinematics formula of the DH model provides a maximum of six independent equations, simplifying the algebraic equation using the numerical method can solve the inverse kinematics problem for most non-redundant robots [7]. In addition, the Jacobian matrix derived in the DH model can be used to solve the inverse kinematics problem for redundant robots [6,8–11]. Based on the numerical method, a series of objective functions can be added [12] that not only enable the robot to reach a designated position but also allow the completion of some extra tasks, such as keeping away from a singular point or maintaining its maximum motion ability. Studies on numerical solutions have primarily focused on eciently and stably solving the equations or obtaining an approximate solution using the functional approximation method in combination with neural networks and intelligent algorithms [11,13,14]. However, owing to the existence of the robotic singular posture, the stability of the numerical solution and its convergence rate cannot be guaranteed. Therefore, this method is not suitable for cases where instantaneity is required. Another method based on the DH model is the analytical method, which uses a closed formula to determine the relations between the terminal posture and various joint angles. Nonetheless, not all robots have a closed-form solution. In 1978, a general method for solving for the closed-form solution was proposed by Paul [15]. In 1968, Pieper studied six-degree-of-freedom (DOF) robots with three axes intersecting at one point [16] and concluded that a serial robot with the three terminal axes intersecting at one point has a closed-form solution. This criterion, i.e., that a six-DOF serial robot with three terminal axes intersecting at one point (deﬁned as Pieper Criterion 1, let us say PC-1) or three adjacent parallel axes (deﬁned as Pieper Criterion 2, let us say PC-2) has a closed-form solution, has been obtained in subsequent studies [17,18]. However, the method proposed by Paul needs to be manually derived and the formula needs to be rederived every time the robot structure changes, which limits the universality of the closed-form solution. Accordingly, studies on the closed-form solution based on the DH model have primarily focused on situations with already known structures, such as keeping the robot away from a singular value [19]. More importantly, the necessary conditions for the existence of the closed-form inverse solution given by Pieper are inadequate. For example, a robot with the DH parameters listed in Table 1 has three terminal axes intersecting at one point, as well as three parallel joints; however, its inverse kinematics problem cannot be solved. Table 1. Counter-example of Pieper Criterion 1 (PC-1). n / d/mm a/mm / 1 d a 0 1 1 1 2 d a 0 2 2 2 3 d a /2 3 3 3 4 d 0 /2 4 4 5 0 0 /2 6 0 0 0 The DH method is a parameter selection norm based on a transformation of coordinates [6]. This method regulates the criterion for selecting three parameters between two adjacent axes, where a represents the linkage oset, i.e., the normal oset between the two adjacent axes, d is the linkage length, representing the axial oset between the two adjacent axes, and is the linkage torsion, representing the included angle between the two adjacent axes. On a similar note, the robot with the DH parameters listed in Table 2 has three adjacent and parallel joints; however, we cannot solve the inverse kinematics problem. Appl. Sci. 2019, 9, 4365 3 of 36 Table 2. Counter-example of Pieper Criterion 2 (PC-2). n / d/mm a/mm / 1 d 0 0 1 1 2 0 a 0 2 2 3 0 a /2 3 3 4 d 0 0 4 4 5 d a 0 5 5 5 6 d 0 0 6 6 The inadequacy of the Pieper principle leads to defects in the inverse kinematics algorithm designed on the basis of this principle. However, no further studies on this problem have been conducted to supplement and improve this method. Conversely, the modeling method for the POE formula has been presented in some detail [20]. The logic for the inverse kinematics solution problem based on POE primarily consists of a reduction of the original problem into several sub-problems followed by solving all the joint angles step by step. This method uses an abstract geometric model to solve the inverse kinematics problem, therefore enhancing the generalization of the algorithm. In POE-based studies, several universal inverse kinematics algorithms have been designed, of which the most typical universal sub-problem was proposed in 1986 [21,22]. There is subproblem 1. Rotation about a single axis, subproblem 2. Rotation about two subsequent axes, and subproblem 3. Rotation to a given distance. A robotic universal solving method that meets the Pieper principle has been proposed [23], and a new sub-problem was proposed to solve the universal inverse kinematics algorithm [24]. The universal inverse kinematics algorithm based on the POE model has deﬁnite geometrical signiﬁcance and stable numerical values. However, in practice, it is impossible for some robots to utilize the known sub-problems to describe and solve the problem. In addition, the algorithm consumes enormous computational resources when selecting sub-problems. As a result, it is dicult for the universal inverse solution algorithm based on the POE model to be applied in cases with real-time requirements. It can be seen that both the DH model and POE model have various drawbacks in solving inverse kinematics problems. This study proposes a method for judging the existence of closed-form solutions based on the DH model to deal with the ﬁrst problem of the closed-form solution. In this method, revolute serial robots with closed-form solutions are described using three types of sub-problems from the viewpoint of solving the algebraic equations. They are sub-problem of translation components, sub-problem of rotational components, and sub-problem of sub-chains. In more detail, based on sub-problems, when the conﬁguration of the robot satisﬁes some characteristics, some more concise and ﬁxed formulas can be used to solve the joint angle. This article refers to this more speciﬁc situation as the basic problem of sub-problems, a total of ten. If a serial robot can be described using the three types of sub-problems, i.e., if the inverse motion problems can be solved by several basic problems, then there is a closed-form solution. Based on the above method, a set of universal closed-form inverse kinematics solving algorithms is designed to address the second problem mentioned above. Since there is a deﬁnite formula solution in the three types of sub-problems, the joint angles can be rapidly determined. In addition, because the DH parameters can directly reﬂect the linkage of the robot, the judgment of the sub-problems is also quick and accurate. In general, the main contributions of this article are: 1. Proposes a more complete judgment condition for the existence of the closed-form solution of series robot to solve the defect of Pieper principle. In addition, this judgment method is a method that is suitable for all robots whose degree of freedom is less than six. 2. A general inverse kinematics algorithm is designed for this judgment method. We can use this algorithm to ﬁnd the angles of all joints of the series robots meet the conditions quickly and accurately. Appl. Sci. 2019, 9, 4365 4 of 36 3. This universal inverse solution algorithm has a simple judgment method, the calculation speed of it is fast, and it is an algorithm that can be applied in occasions with real-time requirements. Usually, a special method is used for the motion control of series robot, only the common Appl. Sci. 2019, 9, x FOR PEER REVIEW 4 of 36 conﬁguration is supported, and some parameters cannot be adjusted. Therefore, the proposed method can greatly improve the application range of the motion controller of series robots. configuration is supported, and some parameters cannot be adjusted. Therefore, the proposed 4. Proposed method and theory has also enriched the foundation of robotics. method can greatly improve the application range of the motion controller of series robots. 4. Proposed method and theory has also enriched the foundation of robotics. The manuscript is organized as follows. Section 2 derives a kinematics equation for a robot based on the standard DH model. Relevant properties are obtained according to the forward kinematics The manuscript is organized as follows. Section 2 derives a kinematics equation for a robot based equation to be used in the inverse kinematics problems. Section 3 analyzes the robotic inverse kinematics on the standard DH model. Relevant properties are obtained according to the forward kinematics problems. According to the existence conditions, the inverse kinematics problems are divided into equation to be used in the inverse kinematics problems. Section 3 analyzes the robotic inverse three sub-problems and 10 types of basic problems. Section 4 designs a universal inverse kinematics kinematics problems. According to the existence conditions, the inverse kinematics problems are algoridivide thm. S d into ection th5 ree sub-pro designedbtlems and wo expe 10 rim types ents. oT f h be asf ii c problems. rst experim Se en ct tion us 4 esdM esA igns TL A a univer B andsa Rlo in bo verse tics Toolbo kinem x [25] atics to t algorithm. est the com Sec ple tt ion 5 eness de , v signe ersad ti two experiments. lity, and continui tThe y of first the a exper lgoriitmen hm.t use Subss e MATL quentAB and ly, the Robotics Toolbox [25] to test the completeness, versatility, and continuity of the algorithm. algorithm is implemented and verified in a real-time system to demonstrate its correctness and real-time Subsequently, the algorithm is implemented and verified in a real-time system to demonstrate its stability. Section 6 presents a summary of the findings of this paper. correctness and real-time stability. Section 6 presents a summary of the findings of this paper. 2. Kinetic Models 2. Kinetic Models 2.1. Standard DH Parameters 2.1. Standard DH Parameters The DH method is a parameter selection norm based on a transformation of coordinates [6]. The DH method is a parameter selection norm based on a transformation of coordinates [6]. Let axis k denote the axis of the axis connecting link k 1 to link k; DH method adopted to deﬁne Let axis 𝑘 denote the axis of the axis connecting link 𝑘− 1 to link 𝑘 ; DH method adopted to link Frame k: define link Frame 𝑘 : Choose axis z along the axis of joint k + 1. Locate the origin O at the intersection of axis z with k k k Choose axis 𝑧 along the axis of joint 𝑘+ 1 . Locate the origin 𝑂 at the intersection of axis the common normal to axes z and z . Locate also O at the intersection of the common normal with k1 k k0 𝑧 with the common normal to axes 𝑧 and 𝑧 . Locate also 𝑂 at the intersection of the common axis z . Choose axis x along the common normal to axes z and z with direction from joint k to k1 k k1 k normal with axis 𝑧 . Choose axis 𝑥 along the common normal to axes 𝑧 and 𝑧 with direction joint k + 1. Choose axis y so as to complete a right-handed frame. from joint 𝑘 to joint 𝑘+ 1 . Choose axis 𝑦 so as to complete a right-handed frame. Once the link frames have been established, the position and orientation of Frame k with respect Once the link frames have been established, the position and orientation of Frame 𝑘 with to Frame k 1 are completely speciﬁed by the following parameters: a distance between O and O k k k0 respect to Frame 𝑘− 1 are completely specified by the following parameters: 𝑎 distance between d coordinate of O along z , angle between axes z and z about axis x to be taken positive k k0 k1 k1 k k 𝑂 and 𝑂 𝑑 coordinate of 𝑂 along 𝑧 , 𝛼 angle between axes 𝑧 and 𝑧 about axis 𝑥 to when rotation is made counter-clockwise, and angle between axes x and x about axis z to be k k1 k k1 be taken positive when rotation is made counter-clockwise, and 𝜃 angle between axes 𝑥 and 𝑥 taken positive when rotation is made counter-clockwise. about axis 𝑧 to be taken positive when rotation is made counter-clockwise. Using the DH method, the relations for a robot can be directly established in a visualized manner Using the DH method, the relations for a robot can be directly established in a visualized manner and the existence conditions for closed-form solutions can be derived in a more direct and visualized and the existence conditions for closed-form solutions can be derived in a more direct and visualized manner. The standard DH parameters in the case of the KingKong collaborative robot are shown in manner. The standard DH parameters in the case of the KingKong collaborative robot are shown in Table 3. A schematic diagram of the robot is shown in Figure 1. Table 3. A schematic diagram of the robot is shown in Figure 1. 𝑗 𝑧 𝑦 𝑥 6 6 3 6 Joint 6 𝑥 4 𝑗 𝑧 𝑥 4 5 Joint 4 Joint 5 𝑥 𝑦 𝑗 5 5 Joint 3 𝑥 2 𝑧 Joint 2 Joint 1 𝑥 𝑦 𝑦 1 1 0 (a) (b) Figure 1. (a) Schematic for the KingKong parameters: 𝑑 is the linkage length of the 𝑘 th link and 𝑎 Figure 1. (a) Schematic for the KingKong parameters: d is the linkage length of the kth link and a is k k is the linkage torsion of the 𝑘 th link. 𝑗 is the 𝑘 th joint. (b) Visualization of the KingKong model. the linkage torsion of the kth link. j is the kth joint. (b) Visualization of the KingKong model. Appl. Sci. 2019, 9, 4365 5 of 36 Table 3. Denavit–Hartenberg (DH) parameters of the KingKong collaborative robot. n / d/mm a/mm / 1 89.459 0 /2 2 0 42.5 0 3 0 39.225 0 4 109.15 0 /2 5 94.65 0 /2 6 82.3 0 0 2.2. Forward Kinematics Formula Using the following formula, the DH parameters [6] can be transformed into elements of a transformation matrix from [6]: 2 3 cos sin cos sin sin a cos 6 k k k k k k k 7 6 7 " # 6 7 6 7 6 7 R P sin cos cos cos sin a sin 6 7 k1 k k k k k k k k k 6 7 ( ) T = = 6 7, (1) k 13 6 7 6 7 0 1 0 sin cos d 6 k k k 7 6 7 4 5 0 0 0 1 k1 k1 where k indicates the number of the current joint. R is the rotational component of T( ), a k k k1 k1 3 3 orthogonal matrix, and P is the translational component of T( ). The end transformation k k matrix of the end executor of the robot relative to the coordinate system can be obtained using the following formula: 0 0 1 2 i1 T = T T T T . (2) i 1 2 3 i i indicates the number of degrees of freedoms of the robot. According to Equation (2) from [18], we could obtain T as follows: 2 3 n=i n=i " # Q P 6 7 0 0 n1 0 i1 6 7 R P 6 R R ::: P 7 0 6 n 7 i i 1 i 6 7 T = = , (3) 6 n=1 n=1 7 13 i 6 7 0 1 4 5 13 0 1 0 0 i1 where R is the rotational component of T . Since the rotational component R is closed and i i i 0 0 0 orthogonal, R is an orthogonal matrix. Here, P is the translational component of T , which can be i i i represented as follows: n=i n=i X X 0 0 i1 P = R ::: P = P . (4) i 1 i n=1 n=1 According to Equation (4), the positional component of the back linkage is inﬂuenced by the front joint angle. In the deﬁnition of the DH parameter, the Z-direction of the joint coordinate indicates the orientation of the joint and can be used to characterize the relationship between the joints. Here, the joints of the serial robots are represented as z . To concisely indicate the linkage relations, the following a,d deﬁnitions are made: = 0, then z k z , whereas = /2, then z ?z , where z represents k k k+1 k k k+1 the linkage parameters a = d = 0. k k 2.3. Decoupling of the End Linkage " # 0 0 R P i1 i1 The coordinate transformation matrix is written as T = . Moreover, the 13 i1 0 1 " # i1 i1 R P i1 i i transformation matrix of the last axis is T = . The rotational component R is written 13 0 1 h i ! ! ! in the form of a row vector as R = x y z . Appl. Sci. 2019, 9, 4365 6 of 36 Then, P can be written as: 2 3 6 cos 7 6 7 6 7 6 7 0 0 0 0 6 7 P = R6 sin 7a + z d + P, (5) i i i 6 7 i i1 i1 i1 6 7 4 5 further: 2 3 cos 6 7 6 7 6 7 6 7 0 0 6 7 R6 sin 7 = x . (6) 6 7 i1 i 6 7 4 5 The following conclusions can be drawn: ! ! 0 0 when = /2 then y = z , i i1 ! ! 0 0 0 0 P = P x a y d ; (7) i i i1 i i i ! ! 0 0 when = /2 then y = z , i i1 ! ! 0 0 0 0 P = P x a + y d ; (8) i i i1 i i i ! ! 0 0 and when = 0 then, z = z , i i1 ! ! 0 0 0 0 P = P x a z d . (9) i i i1 i i i The above derivation demonstrates that when the current end transformation matrix T is known, 0 0 the translational component P in the previous transformational matrix T can be obtained based i1 i1 0 0 0 on the rotational component R of T and the translational component P. Given such a property, the i i i last axis may not be considered when the translational component of the robot is being analyzed. 2.4. Translational Relation of the Rotational Component k1 According to Equation (3) form [6], R = Rotz( )Rotx( ). The rotational component of the k k transformational matrix can be expressed as: n=k n=k Y Y 0 n1 R = R = Rotz( )Rotx( ). (10) n n n=1 n=1 If there exists several continuous parallel joints in the robot, then z k z ::: k z and = l l+1 l ::: = = 0. In addition, the rotational transformation in the same direction has additivity: l+1 m1 n=m R = Rotz( )Rotx( ) = Rotz(Q)Rotx( ) = R , (11) n n m n=l where Q = + ::: + . Here, we refer to z k z ::: k z as a sub-chain s. This concept will be m m l l+1 l l+1 used in the following paragraphs. According to Equation (11), the translational components of the sub-chain z k z ::: k z can be deﬁned and simpliﬁed as follows: 2 m 2 3 2 3 2 3 6 a 7 6 0 7 6 a cos Q 7 m m 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 m1 6 7 6 7 6 7 P = R P = Rotz(Q)6 0 7 + 6 0 7 = 6 a sin Q 7 = P , (12) m m m 1.m1 6 7 6 7 6 7 s 6 7 6 7 6 7 4 5 4 5 4 5 0 d d m m where Q = + ::: + . 2 m 1 Appl. Sci. 2019, 9, 4365 7 of 36 R deﬁned by the formula (11) and P deﬁned by the formula (12), will be used in the following s s paragraphs. 2.5. Solving the Trigonometric Equation This section summarizes two common formula solutions for trigonometric functions. The ﬁrst can be derived using Equation (4): 2 3 1 1 1 6 P sin n P sin + P a + a cos 7 y 1 1 x 1 z 1 1 1 n=i 6 7 X i i i 6 7 6 7 0 0 0 1 n1 6 1 1 1 7 6 7 P = P + R R P = P a + a sin n P n P cos , (13) 6 z y x 7 n 1 1 1 1 1 1 i 1 1 6 i i i 7 6 7 4 5 1 1 1 n=2 sin P + n P + 1 + P d 1 x 1 y z 1 i i i 1 1 1 0 1 0 0 ( ) where P , P , and P represent the three components in R P P and n = sign . According x y z i i i i i 1 i 1 0 0 to Equation (18), P , P , and satisfy a special trigonometric function relation, as shown in x y 1 i i Equation (14): " # " #" # P D L sin x 1 = , (D , 0 or L , 0). (14) P L D cos Therefore, 0 0 0 0 = atan2 D P + L P , L P D P . (15) 1 x y x y i i i i Another common trigonometric equation is shown in Equation (16) from [17]: Acos + Bsin = C, (A , 0 or B , 0), (16) whose solution is: 0 1 2 2 2 B C B B + A C B C B C = 2atanB C. (17) @ A A + C Equations (15) and (17) can map the joint angles within the interval of [, ]. Moreover, in the following paragraphs, the robotic inverse solution equation is transformed into Equations (14) and (16), resulting in a ﬁxed formula, which can compute the equation parameters and further derive the common closed-form formula solution for serial robots. When the parameters for Equations (14) and (16) are zero, the equation has no solution. This provides a suitable answer to the question of whether a robot has a solution when the linkage parameters are sparse. For convenience, Equation (14) is represented as = F (D, L, x, y) and Equation (15) is represented as , = F (A, B, C). 1 1 k1 k2 2 3. Analysis of The Inverse Kinematics Problem " # 0 0 R P i i Given a transformational matrix with a known end T = , the joint angle is obtained 13 0 1 0 0 inversely from the equation provided by R and P; this is known as inverse kinematics. i i 3.1. Three Types of Sub-Problems Related to Closed-Form Inverse Kinematics The French mathematician Évariste Galois proved that the quartic polynomial equation has a closed-form solution, whereas the sextic polynomial equation generally has none. The rotational and translational components of the robotic end transformational matrix provide three independent equations separately. The two equation sets are combined to solve the inverse kinematics problem, and then high-order polynomial equations may be required. Therefore, a robot has a closed-form inverse solution, partial joint angles will be preferably solved from a certain equation set followed by the other joint angles. To guarantee the existence of a closed-form solution, a feasible solution guarantees mutual decoupling between the translational and rotational components [17,26]. Such traditional decoupling Appl. Sci. 2019, 9, 4365 8 of 36 enables the translational component to be associated with the front three joints, which in turn, are 0 3 solved via P. The latter three joints are solved via the rotational component R. 3 6 " #" # " # 0 0 3 0 3 0 R P R 0 R R P 3 3 6 3 6 3 T = = . (18) 0 1 0 1 0 1 Based on the derivation in Section 2.4, a robot has several parallel joints, the number of unknown numbers in the rotational component will decrease. Therefore, another feasible decoupling idea is to preferably ﬁnd a solution using the rotational component R(',#, ) and solve for the remaining joint angles using the translational component P. This strategy has not been mentioned in any previous studies. " # " # 0 0 0 0 R P R(',#, ) P 6 6 6 6 T = = . (19) 0 1 0 1 Based on the above two decoupling methods, we could obtain three types of sub-problems. In the ﬁrst type, when the number of unknowns in the rotational component is larger than three and the robot meets some structural characteristics, some joint angles can be preferably and directly solved using the translational component. This article refers to the ﬁrst type sub-problem as the sub-problem of translation components. In the second type, the robot has several parallel joints and the number of unknowns in the rotational component is no greater than 3, these unknowns can be solved using the rotational component. This article refers to the second type sub-problem as sub-problem of rotational components. In the third and final type, when the second type is used to solve the unknowns for the robot and the joint angles are still not completely solved, the remaining joint angles are obtained using the translational component. This article refers to the third type sub-problem as a sub-problem of sub-chains. Via an analysis of these sub-problems, it is possible to determine the solving conditions and methods in detail. In Sections 3.2–3.4, a more detailed analysis and derivation will be made for each sub-problem, and the formulas and conditions for solving the basic problems will be obtained. 3.2. The First Type of Sub-problem When there exist more than three unknowns in the rotational component, the partial joint angles should preferably be solved using the translational component. With reference to the DH parameter table, the DOF of the robot is i and the number of the DOF with = 0 in the ﬁrst i 1 DOFs is p. In the case of i p > 3, there are more than three unknowns in the rotational component. According to Equation (4), the back joint angles are inﬂuenced by the front joint angles in the translational component. Therefore, the association with a maximum of the front three joint angles in the translational equation must be ﬁrst guaranteed to solve the ﬁrst type of sub-problem. When the ﬁrst joint angle of the robot model Robot with i DOFs is solved, T can be calculated using the forward kinematics formula. Meanwhile, the ﬁrst row of the DH parameters is deleted to form a new robot model Robot and a new transformational matrix T : i1 i1 " # 0 1 0 1 0 R R P 1 0 1 0 0 1 1 1 T = T T = T . (20) i 1 i 13 i 0 1 There may only be a parallel or vertical relationship between two adjacent joints. Since only the linkage relation of the front three axes is considered, the possible conﬁguration between the ﬁrst three 1 1 axes is only the case where C C = 4. Moreover, z ?z ?z , z k z ?z , z ?z k z and z k z k z . 1 2 3 1 2 3 1 2 3 1 2 3 2 2 However, during the process of derivation, it was found that z k z k z only provides two valid 1 2 3 equations in X-direction and Y-direction, making it impossible to solve for three unknowns: Appl. Sci. 2019, 9, 4365 9 of 36 2 3 6 a cos( + + ) + a cos( + ) + a cos 7 3 1 2 3 2 1 2 1 1 6 7 6 7 6 7 6 7 P = a sin( + + ) + a sin( + ) + a sin . (21) 6 7 3 1 2 3 2 1 2 1 1 6 7 6 7 4 5 d + d + d 1 2 3 Such a case is the counter example mentioned in the preceding paragraphs. The Piper principle does not speciﬁcally describe such a case. Therefore, the linkage parametric limitations and the formula solution for the three basic problems are derived in detail in the following paragraphs. 3.2.1. First Three Joint Conﬁgured as z k z ?z 1 2 Here, the expression for the translational component is P = P + R P + R R P . However, 1 1 2 1 2 3 R R P contains the cubic term of the sine and cosine functions, which would directly increase the 1 2 3 order of the equation to sextic. Only when a = 0 will the requirements for a closed solution be met. According to Equation (18), The formula solution of can be obtained as follows: 0 0 > D = n d = F D, L, P , P 1 2 11 1 x y < 3 3 , where . (22) 0 0 0 2 0 2 2 L = P + P D = F D,L, P , P 12 1 x y x y 3 3 3 3 Once is obtained, the forward kinematics formula can be used to forward simplify the robot to obtain the expression of T and the DOFs of i 1. The basic problem can still be employed to seek a solution to . To guarantee the existence of only the front two joint angles in the translational equation, the linkage of z needs to satisfy d = a = 0. However, according to the end decoupling formula proposed k>3 in Section 2.3, the linkage parameters of the last joint are not restricted. 3.2.2. First Three Joint Conﬁgured as z k z ?z 1 2 3 The formula solution for z k z ?z is derived in the following paragraphs. Its translational 1 2 3 component can be written as P = P + P + R P , and its translational equation can be arranged as 1 12 12 3 follows: 2 3 2 3 6 a cos + D sin( + ) + L cos( + ) 7 6 P 7 1 1 1 2 1 2 x 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 a sin + L sin( + ) D cos( + ) 7 = 6 P 7. (23) 2 2 y 1 1 1 1 6 7 6 7 6 7 6 7 4 5 4 5 n a sin P d d 2 3 3 z 1 2 Arranging Equation (23) gives the formula solution Equation (24): A = 0 > 3 , = F (A , B , C ), where B = n a ; 31 32 2 3 3 3 3 2 3 C = P d d 3 1 2 A = P 1 x > 3 > B = P > 1 y D = n d 2 3 , = F (A , B , C ), where 11 12 2 1 1 1 (24) L = a + a cos 2 3 3 0 2 0 2 2 2 2 P + P +a L D > x y 3 3 C = 2a > g 0 0 P = P a cos x x 1 1 0g 0g 3 3 + = F D, L, P , P , where ; 1 2 1 x y 3 3 > : 0 0 P = P a sin y y 1 1 3 3 = f[( + ) ] ; 2 1 2 1 where f[] in the last equation indicates that is mapped onto the interval of [,]. Under such a structure, is preferably solved using the equation in the Z-direction. Therefore, the existence of z after z does not inﬂuence the solution of , resulting in the derivation of two 3 3 a a dierent structures, z k z ?z ?z and z k z ?z k z . 2 3 2 3 1 1 4 4 Appl. Sci. 2019, 9, 4365 10 of 36 According to the translational equation of z k z ?z ?z and Equation (23), the critical coefficient is: 1 2 3 D = n d + n d 2 3 2 4 . (25) L = a + a cos 2 3 3 Moreover, according to Equation (23), the critical coecient under z k z ?z k z is: 1 2 3 D = n d 2 3 A = n n d , where (26) 3 2 3 4 L = a + a cos + n d sin . 2 3 3 3 3 To guarantee that only the front three joint angles exist in the translational equation, the linkage of z needs to satisfy d = a = 0. There is not restricted to the linkage parameters of the last joint. k>4 In addition, in the basic problems mentioned above, when d = a = 0 is true in z , it can also 4 4 4 be used to solve z k z ?z . Similarly, it can be used to solve z k z . Therefore, such a type of basic 1 2 3 1 2 problem contains two sets of formula solutions that can be used to solve four cases. 3.2.3. First Three Joint Conﬁgured as z ?z k z 1 2 3 a a The solution method for z ?z k z in Section 3.4.1 also applies here. In addition, if z ?z k z ?z 1 2 1 2 3 3 4 and D = n (d + d ), can still be directly solved. This results in a special situation. Further, 1 2 3 1 suppose there is a robot with i degrees of freedom, in which the conﬁguration of the ﬁrst g joints is z ?z k ::: k z ?z , and the linkage of z needs to satisfy d = a = 0. These are not restricted to the 1 2 g1 k>g linkage parameters of the last joint. The formula solution of can be obtained as follows: g1 > P 0 0 > > D = n d = F D, L, P , P m 11 1 x y < g g , where m=2 . (27) 0 0 = F D,L, P , P 12 1 x y > g g 0 0 2 2 2 L = P + P D x y g g According to the forward kinematics formula, after is solved, T can be cancelled to allow the robot model to reduce a joint angle and become a new model. At that time, selective solving can be performed in all of the sub-problems. Basic problems 3.2.1 and 3.2.3 can be solved using the same formula solution. In the following paragraphs, basic problem 3.4.1 will be used for the description. Therefore, there exist two types of basic problems in the ﬁrst type of sub-problem. This section gives the corresponding relevant existence conditions and formula solutions. 3.3. Solving the Second Type of Sub-Problem Since the rotational component only provides three independent equations, a maximum of three unknowns can be solved. First, the rotational matrix Rotx() is introduced. Rotx() represents a radian rotation around the X-coordinate axis. Similarly, Rotz() and Roty() represent rotations around the Y- and Z-axes. The Euler angle theorem indicates that a generic rotation matrix can be obtained by composing a suitable sequence of three rotations while guaranteeing that two successive rotations are not made around parallel axes [18]. Therefore, any rotational matrix R can be expressed as Rotz('), Roty(#), and Rotz( ). The DH parameters of a serial robot with a certain DOF are shown in Table 4. Table 4. The DH parameters of a serial robot with three degrees-of-freedoms (DOFs). n / d/mm a/mm / 1 d a /2 1 1 1 2 d a /2 2 2 2 3 d a 0 3 3 3 Appl. Sci. 2019, 9, 4365 11 of 36 According to the forward kinematics formula, R = Rotz( )Rotx( )Rotz( )Rotx( )Rotz( )Rotx( ), 1 1 2 2 3 3 (28) R = Rotz( )Roty( )Rotz( ). 1 2 3 A serial robot has three continuous joints meeting = , = and = 0, whose rotational 1 2 3 2 2 components R are known, two sets of solutions can be found using Euler ’s formula. 3.3.1. Three Mutually Perpendicular Sub-Chains Based on in the DH parameters, the conclusions in Section 2.4 can be used to simplify the sub-chains and obtain the expression of the rotational matrix, 2 3 6 cos cos cos + n n sin sin n n sin cos cos cos sin n cos sin 7 s s s s e s e s s s e s e s s s s s s e s s 6 7 1 2 3 1 2 1 3 1 2 1 3 1 2 3 2 1 2 6 7 6 7 6 7 R = 6 sin cos cos n n cos sin n n cos cos sin cos sin n sin sin 7, (29) s s s s e s e s s s e s e s s s s s s e s s 6 1 2 3 1 2 1 3 1 2 1 3 1 2 3 2 1 2 7 6 7 4 5 n sin cos n sin sin n n cos s e s s s e s s s e s e s 1 2 3 1 2 3 1 2 2 for which the following expressions can be obtained: = atan2(n r , n r ) s s e 23 s e 13 1 2 2 > p 2 2 = atan2 r + r ,n n r , (30a) > s s e s e 13 23 33 > 2 1 2 = atan2(n r , n r ) s s e 32 s e 31 3 1 1 or = atan2(n r ,n r ) s s e 23 s e 13 > 1 2 2 2 2 > = atan2 r + r ,n n r , (30b) s 13 23 s e s e 33 > 2 1 2 = atan2(n r ,n r ) s s e s e 32 31 3 1 1 where represents the sum of the rotation angles of the kth sub-chain of the robot and n represents s s e k k the symbol of the last linkage in the kth sub-chain. r represents the element at the position of the xy xth row and the yth column of the rotation matrix R. The problem of solving the sum of the rotation angles of three mutually perpendicular sub-chains is referred to as basic problem 3.3.1. 3.3.2. Two Mutually Perpendicular Sub-Chains Similarly, when there are only two mutually perpendicular sub-chains, the expression of the rotational component at that time is as follows: 2 3 6 cos cos cos sin n sin 7 s s s s s e s 6 1 2 1 2 1 1 7 6 7 6 7 6 7 R = 6 sin cos sin sin n cos 7, (31) s s s s s e s 2 6 1 2 1 2 1 1 7 6 7 4 5 n sin n cos 0 s e s s e s 1 2 1 2 with the corresponding solution: = atan2(n r , n r ) > s s e 31 s e 33 2 1 1 . (32) r r > 21 11 : = atan2 , cos cos s s 2 2 r represents the element at the position of the xth row and the yth column of the rotation matrix xy R. The problem of solving the sum of the rotational degrees of two mutually perpendicular sub-chains is referred to as basic problem 3.3.2. 3.4. The Third Type of Sub-Problem Before the second type of sub-problem is employed by the governing algorithm, the DOF of the robot at that time is i, then the number of DOFs with = 0 in the ﬁrst i 1 DOFs is p. If p , 0, not all Appl. Sci. 2019, 9, 4365 12 of 36 joint angles are obtained by the second type of sub-problem. In this case, the translational component needs to be employed to solve for the remaining joint angles. Such problems are called the third type of sub-problem. For any of this type of sub-problem, when the ﬁrst chain only has one joint, the forward kinematics formula can be directly used to simplify the robotic model. Based on that and according to Equation (4), the translational components of a robot with a DOF of i can be divided into i components. Since the second type of sub-problem can solve the sum of the rotational angles of the sub-chains of the robot, based on Equation (17), the translational equation can be arranged as n=i 0 n1 P P R P R R P = P P R P R R P . (33) s s s s s s n s s s s s s 1 1 2 1 2 3 1 1 2 1 2 3 n=1 P and P represent the rotational and translational components of the kth sub-chain, which is s s k k deﬁned by the formula (11) and formula (12). All terms on the left side of Equation (33) are known and are written as P in the following paragraphs. Conversely, there are unknown quantities on the right side of the equation. Since the third type of problem has a maximum of three sections of sub-chains, [s , s , s ] is used to represent the length of each sub-chain on the right side of the equation. Depending 1 2 3 on the length of each sub-chain, the appropriate formula solutions are presented with s t representing the tth joint in the ith section of the sub-chain. As an example, for z k z ?z k z , there exist two sub-chains, s and s , either of which has a 1 2 3 4 1 2 sub-chain length of two. After the second type of sub-problem is solved, the length of each sub-chain decreases by one. For example, the length of each sub-chain on the right side of Equation (33) is [1, 1, 0]. All possible cases will be analyzed and solved for in the following cases. 3.4.1. Sub-Chain Length as [1,0,0] ˆ ˆ = atan2 a P , a P . s 1 s 1 y s 1 x 1 1 1 h i (34) = . s 2 s s 1 1 1 3.4.2. Sub-Chain Length as [2,0,0] > ˆ > A = P < ˆ B = P , = F (A, B, C), where . s 11 s 12 2 1 1 > 2 2 2 2 ˆ ˆ P +P +a a x y s 1 s 2 : C = 2a s 1 e ˆ P = P a cos x x s 1 s 1 > 1 1 > f ˆ (35) < P = P a sin y y s 1 s 1 e f 1 1 + = F D, L, P , P , where > s 1 s 2 1 x y 1 1 > L = a > s 2 D = 0 h i = f + s 2 s 1 s 2 s 1 1 1 1 1 h i = f + s 3 s s 1 s 2 1 1 1 1 3.4.3. Sub-Chain Length as [2,1,0] and [1,1,0] A = 0 , = F (A, B, C), where B = n a . (36a) s 11 s 12 2 s e s 1 2 2 1 2 C = P d d z s 1 s 2 1 1 h i = f . (36b) s 2 s 1 2 1 1 Appl. Sci. 2019, 9, 4365 13 of 36 P R P = P + R P . (36c) s 1 s 1 s 1 s 2 1 2 1 k k R and P represent the rotational and translational components of the tth joint in the kth section s t s t k k of the sub-chain. According to Equations (36a) and (36b), all joint angles in the sub-chain s can be solved. Moreover, according to Equation (36c), the ﬁrst sub-chain solving problem is transformed into either basic problem 3.6.1 or basic problem 3.6.2. 3.4.4. Sub-Chain Length as [1,2,0] A = n a sin > s s 2 s 2 > 1 1 1 B = n a sin ( ) , = F A, B, C , where > s 2 s 1 s . (37a) s 11 s 12 2 1 1 2 1 1 > : ˆ C = R P d d s s 2 s 1 1 2 2 h i = f . (37b) s 2 s s 1 1 1 R P P = P + R P . (37c) s s 1 s 1 s 1 s 2 1 2 2 2 According to Equations (37a) and (37b), we could solve all joint angles in the sub-chain s . According to Equation (37c), the second sub-chain solving problem can be transformed into basic problem 3.4.2. 3.4.5. Sub-Chain Length as [2,0,1] or [1,0,1] A = 0 , = F (A, B, C), where B = n a sin . (38a) s 11 s 12 2 s e s 1 s 3 3 1 3 3 C = P d d + n n d cos z s 1 s 2 s 3 s 1 s 1 s 1 1 1 2 3 3 h i = f . (38b) s 2 s 1 3 3 3 P R R P = P + R P . (38c) s s s 2 s 2 s 1 s 1 1 2 3 2 2 2 According to Equations (38a) and (38b), all joint angles in the sub-chain s can be solved. According to Equation (38c), the ﬁrst sub-chain solving problem can be transformed into either basic problem 3.4.2 or basic problem 3.4.1. 3.4.6. Sub-Chain Length as [1,0,2] > A = 0 , = F (A, B, C), where B = n a sin . (39a) > s s 21 s 22 2 s 1 s 1 1 1 > 2 1 2 T T C = (R R P) d d + n n d cos s s s 1 s 2 s 2 s 1 s 1 s 2 1 2 z 3 3 1 2 1 h i = f . (39b) s 1 s s 2 1 1 T T T T R R P R R P = P + R P . (39c) s s s s s 1 s 1 s 1 s 2 2 1 2 1 1 3 3 3 According to Equations (39a) and (39b), we could solve all joint angles in the sub-chain s . According to Equation (39c), the third sub-chain solving problem can be transformed into basic problem 3.4.2. Appl. Sci. 2019, 9, 4365 14 of 36 In the case of [1,1,1], it may be impossible to ﬁnd a closed-form solution due to the high complexity of the equation. Therefore, the third type of sub-problem has six basic problems. 3.5. Summary The above-mentioned content proposed the two decoupling methods of series robots ﬁrstly. Three types of sub-problems are proposed through these two decoupling methods. Ten basic problems were derived through analyzing the solvable condition and solution method of each type of sub-problems speciﬁcally. If a series robot can be described through the three types of sub-problem based on these ten basic problems, the robot must be a closed inverse solution. P represents the number of the DOF with = 0 in the ﬁrst i 1 DOFs, i.e., the number of = 0 in the ﬁrst i 1 rows of the DH parameter table. In the case of i P > 3, it means that there are more vertical joints in the robot. There will be more than three unknown numbers that cannot be solved in the rotational component. If there is a closed solution, a low-order trigonometric function equation can only be solved through translation components. There are two basic problems that are feasible. The restrictions and solution formula of the two basic problems is shown in Table 5. Table 5. The restrictions and solution formula of the two basic problems. Base Problem Conﬁguration Restrict Solution Formula 1. The conﬁguration of the ﬁrst g joints is z ?z k ::: k z ?z , 1 2 g1 g Equation (30) can be used to 3.2.1 2. The linkage of z needs to satisfy d = a = 0.. k>g solve 3. The linkage parameters of the last joint are not restricted. Equations (25) and (26) can be 1. The conﬁguration of the ﬁrst three joints is z k z ?z , 1 2 3 used to solve critical coecient, 3.2.2 2. The linkage of z needs to satisfy d = a = 0. k>4 formula (24) can be used to solve 3. The linkage parameters of the last joint are not restricted. the angle of ﬁrst three joints. In the case of i P < 3, it means that there are more parallel joints in the robot. There will be less than three unknown numbers in the rotational component, then, the unknown number can be solved through in the rotational component directly. The parallel joints in the rotation component may be an independent joint angle or the sum of the rotation angles of the sub-chains; therefore, they can be represented as unknown number here. In the case of i P = 3, the formula (30) can be used to solve the three-unknown number. In the case of i P = 2, the formula (32) can be used to solve the two-unknown number. In the case of i P = 1, the ﬁrst formula in the formula (32) can be used to solve the only unknown number. If all joint angles are not solved after using the second sub-problems, it indicates that there is long sub-chain in the translational component. At this time, the remaining joint angles need to be solved by combing the third sub-problems with the result of the second sub-problems. The six basic problems in the third sub-problems are shown in Table 6. Table 6. The six basic problems in the third sub-problems. Base Problem Remaining Length of Each Sub-Chain Solution Formula 3.4.1 [1,0,0] Equation (34) 3.4.2 [2,0,0] Equation (35) 3.4.3 [2,1,0], [1,1,0] Equation (36) and Base problem 3.6.2. 3.4.4 [1,2,0] Equation (37) and Base problem 3.6.2. 3.4.5 [2,0,1], [1,0,1] Equation (38) and Base problem 3.6.2 or 3.6.1. 3.4.6 [1,0,2] Equation (39) and Base problem 3.6.2. Since the third type of problem has a maximum of three sections of sub-chains, [s , s , s ] is used 1 2 3 to represent the length of each sub-chain on the right side of the equation. Depending on the length of Appl. Sci. 2019, 9, 4365 15 of 36 each sub-chain, the appropriate formula solutions are presented with s t representing the tth joint in the kth section of the sub-chain. 4. Algorithm Design for Universal Inverse Kinematics The translational and rotational components of a serial robot with a closed-form solution are mutually decoupled. Therefore, the partial joint angles of the robot can be solved based on the ﬁrst or second type of sub-problem. This simpliﬁes the original robot model into a new robot model. If the new robot model has a closed-form solution, the solution will deﬁnitely continue along one of the three types of sub-problems until all the problems are solved. Therefore, the process of solving the closed-form inverse solution of a serial robot can be completed via constant model simpliﬁcation. Appl. Sci. 2019, 9, x FOR PEER REVIEW 16 of 36 The logical ﬂow chart of the entire algorithm is shown in Figure 2. Start i DOF No Yes i – p > 3 Solving the second Decoupling of types of sub-problems the end linkage simplify the robot Yes No p≠0 Meeting the No Yes Configuration restrict of table 5 End Calculate the remaining length of each subchain The first type of No solution sub-problems Yes Is the case that exists No in Table 6 The third type of No solution sub-problems End Figure 2. Logic for the universal inverse kinematics algorithm. Figure 2. Logic for the universal inverse kinematics algorithm. In In Figure Figure 2,2, P r Pepr represen esents the ts th number e number of of the the DOF w DOF with ith = α=0 0 in the in ﬁrst the if irs 1t DOFs, 𝑖− 1 DO i.e.,Fs, thei.e., number the number of α=0 in the first 𝑖− 1 rows of the DH parameter table. of = 0 in the ﬁrst i 1 rows of the DH parameter table. The inverse kinematics problem has multiple solutions. Nonetheless, the selection of these The inverse kinematics problem has multiple solutions. Nonetheless, the selection of these solutions solutions necessitates a relevant upper logic design. In addition, changes in the robotic joints must be necessitates a relevant upper logic design. In addition, changes in the robotic joints must be continuous: continuous: arg min . (40) (40) 𝑚 i−𝜃 ,cur . i,k , , i=1 In cases where the robot is in a stationary state, the joint angle of the 𝑘 set can be first solved and then Equation (40) can be used to determine the inverse motion index value corresponding to the current position. The main purpose of this paper was to find a more complete inverse solution judgment condition, which is more accurate and specific than the Pieper principle. The general inverse solution algorithm designed based on this judgment condition is also directed to a series robot with a closed inverse solution. For this paper purposed ten basic problems, which means a series of robots with closed inverse solutions can be obtained through combining the basic problems. For example, although 𝑧 ∥𝑧 ⊥𝑧 ∥𝑧 ⊥𝑧 ⊥𝑧 ,or 𝑧 ∥𝑧 ⊥𝑧 ∥𝑧 ∥𝑧 ∥𝑧 are not common, but it is indeed that there are robots with a closed inverse solution. In addition, maintaining the robot configuration does not change, only the direction of rotation and the positive or negative of the offset are changed, the algorithm can still calculate correctly. This can provide great convenience for the application of 𝜃 𝑖𝑛 𝑎𝑟𝑔 Appl. Sci. 2019, 9, 4365 16 of 36 In cases where the robot is in a stationary state, the joint angle of the k set can be ﬁrst solved and then Equation (40) can be used to determine the inverse motion index value corresponding to the current position. The main purpose of this paper was to ﬁnd a more complete inverse solution judgment condition, which is more accurate and speciﬁc than the Pieper principle. The general inverse solution algorithm designed based on this judgment condition is also directed to a series robot with a closed inverse solution. For this paper purposed ten basic problems, which means a series of robots with closed inverse solutions can be obtained through combining the basic problems. For example, although a,d z k z ?z k z ?z ?z , or z k z ?z k z k z k z are not common, but it is indeed that there are robots 1 2 3 6 1 2 3 4 5 6 4 5 with a closed inverse solution. In addition, maintaining the robot conﬁguration does not change, only the direction of rotation and the positive or negative of the oset are changed, the algorithm can still calculate correctly. This can provide great convenience for the application of an engineer. Finally, the logical design of the algorithm is complete. This means that once you use a situation other than ten basic questions to design the robot, the algorithm will jump out and terminate. Therefore, the algorithm is not to solve the inverse solution problem of any robot, but to judge whether the closed inverse solution exists and to solve it. 5. Experiments The previous section discussed the design of the universal inverse kinematics solution algorithm. In this section, four of the experiments will be carried out to verify the completeness, versatility, stability, and real-time performance of the algorithm. Frist of all, the algorithm was implemented in MATLAB 2016b on a 64-bit Windows 10 operating system. The completeness, versatility and stability of the algorithm will be veriﬁed in MATLAB. Subsequently, the algorithm was performed using the Beckho-C6920 controller. We built a six-DOF KingKong robot using the Kollmorgen RGM motor for the real-time testing. 5.1. Veriﬁcation Experiment for Algorithm Completeness Firstly, the algorithm designed in this paper was logically complete. The ﬁrst simulation experiment would test the completeness of the algorithm. Here, a robot with three parallel joints was selected as the experimental object. The DH parameter of it is shown in the Table 7. This robot is a common series robot, but it cannot be described by a single sub-problem. Table 7. The DH parameters of the robot with three parallel joints. n / d/mm a/mm / 1 0.1 0.1 0 2 0.1 0.2 0 3 0.1 0.3 0 First, the algorithm receives the DH parameters of the robot. Algorithm will enter in the second type of sub-problem to solve the rotation angle and of the sub-chain s . The length of the sub-chain s 1 was [2, 0, 0] after been simpliﬁed and calculated, therefore, the remaining joint angles was solved by the basic problem 3.6.2 in the third type sub-problems. 2 3 6 90 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 90 7, and the end posture was: 6 7 6 7 4 5 2 3 0.7071 0.7071 0 0.0121 6 7 6 7 6 7 6 7 6 7 0.7071 0.7071 0 0.1121 6 7 6 7 6 7. (41) 6 7 6 7 0 0 1 0.3 6 7 6 7 4 5 0 0 0 1 Appl. Sci. 2019, 9, 4365 17 of 36 All joint angles were solved with the algorithm, as shown in Table 8. Table 8. Two inversely solved joint angle sets. No. / / / 1 2 3 1 90 90 45 2 36.8699 90 8.1301 It is easy to verify that the two sets of joint angles are correct. A robot with four parallel joints was now used as the experimental object. Its DH parameters are shown in Table 9. Table 9. The DH parameters of the robot with three parallel joints. n / d/m a/m / 1 0.1 0.1 0 2 0.1 0.2 0 3 0.1 0.3 0 4 0.1 0.1 0 The algorithm will enter in the second type of sub-problem the solve the rotation angle and of the sub-chain s . The length of the sub-chain was [3,0,0] after being simpliﬁed and calculated, the algorithm was ended for it was not in the basic problem of the third type sub-problems. In fact, there are only three valid equations for series robots with four-degree-of-freedom, which is impossible to solve its inverse motion problem. Experiment 5.1 veriﬁed the completeness of the algorithm by two examples. The ﬁrst example was a three-parallel joint robot, the correct joint angles were obtained through an algorithm. Then, we used a four-parallel joint robot to test, and the algorithm was terminated for it was not in the basic problems. This shows that the algorithm would exit correctly when it encounters an unsolvable problem, and it will be able to calculate the result when it encounters a solvable problem. This experiment also proved that the proposed judgment method could be used to low-degree- of-freedom robots. In addition, when there were four adjacent parallel joints in a six-degree-of-freedom robot, the Pieper criterion was satisﬁed, but the proposed algorithm could judge that the robot did not have a closed inverse solution. This complements the shortcomings of the Pieper criterion. 5.2. Veriﬁcation Experiment of Algorithm Versatility The proposed algorithm could solve those serial robots that could be described by three types of sub-problems. Since the description of the ten basic problems was determined, a series robot with a closed-form inverse solution could be constructed based on ten basic problems. Two uncommon robots were used as examples to verify the versatility of the algorithm in this experiment. a,d We constructed a robot Bot , whose structure was z k z ?z k z ?z ?z based on basic problem 1 1 2 3 6 4 5 3.4.2 and 3.5.1. Its DH parameters are shown in Table 10. Table 10. The DH parameters of Bot . n / d/m a/m / 1 0.1 0.35 0 2 0.1 0.3 /2 3 0.1 0.5 0 4 0.1 0 /2 5 0 0 /2 6 0.1 0.1 0 6 Appl. Sci. 2019, 9, 4365 18 of 36 The robot had three vertical joints, so the decoupling of the end was carried out ﬁrstly. Then, we a,d found that z k z ?z k z ?z could be solved by the basic problem 3.2.2. When the ﬁrst three joint 1 2 3 4 5 a,d angles were solved, the simpliﬁed robot z ?z ?z could be solved by the second type of sub-problems. 4 5 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end posture was: 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 2 3 0.5625 0.1752 0.8080 0.0228 6 7 6 7 6 7 6 7 6 7 0.8248 0.1875 0.5335 0.6441 6 7 6 7 6 7. (42) 6 7 6 7 0.0580 0.9665 0.2500 0.7192 6 7 6 7 4 5 0 0 0 1 All joint angles were solved with an algorithm, as shown in Table 11. Table 11. Four inversely solved joint angle sets. No. / / / / / / 1 2 3 4 5 6 1 90.0000 60.0000 90.0000 120.0000 30.0000 150.0000 2 90.0000 60.0000 90.0000 60.0000 30.0000 30.0000 3 116.7078 7.3801 90.0000 177.4728 14.4919 89.1752 4 116.7078 7.3801 90.0000 2.5272 14.4919 90.8248 It is easy to verify that the four sets of joint angles are correct. Then, we kept the conﬁguration of the robot unchanged and modiﬁed the positive direction of the rotation of the robot joint, the positive and negative of a and the value of d. This was used to verify whether the algorithm could cope with parameter changes. The changed DH parameters are shown in Table 12. Table 12. The DH parameters of Bot after changing the parameters. n / d/m a/m / 1 0 0.35 0 2 0 0.3 /2 3 0 0.5 0 4 0 0 /2 5 0 0 /2 6 0 0.1 0 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end posture was: 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 2 3 0.5625 0.8248 0.0580 0.2036 6 7 6 7 6 7 6 7 6 7 0.1752 0.1875 0.9665 0.1825 6 7 6 7 . (43) 6 7 6 7 6 7 0.8080 0.5335 0.2500 0.4192 6 7 6 7 4 5 0 0 0 1 All joint angles were computed with the algorithm, as shown in Table 13 Appl. Sci. 2019, 9, 4365 19 of 36 Table 13. Four inversely solved joint angle sets. No. / / / / / / 1 2 3 4 5 6 1 90.0000 60.0000 90.0000 120.0000 30.0000 150.0000 2 90.0000 60.0000 90.0000 60.0000 30.0000 30.0000 3 14.8218 60.0000 90.0000 105.2403 108.0015 51.7522 4 14.8218 60.0000 90.0000 74.7597 108.0015 –128.2478 It is easy to verify that the four sets of joint angles are correct. We constructed the robot Bot , whose structure was z k z ?z k z k z k z based on basic 2 1 2 3 4 5 6 problem 3.5.1 and 3.6.6. Its DH parameters are shown in Table 14. Table 14. The DH parameters of Bot . n / d/m a/m / 1 0.1 0.35 0 2 0.1 0.3 /2 3 0.1 0.3 /2 4 0.1 0.25 0 5 0.1 0.2 0 6 0.1 0.1 0 The robot had three sub-chains, and the second type of sub-problem was ﬁrstly used to obtain the sum of the rotation angle of the three segments. Then, algorithm turned into the third type sub-problems for there were joint angles that had not been solved. In the third sub-problem, we could use the basic problem 3.6.6 to solve, and then solve all joint angles. 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end posture was: 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 2 3 0.0474 0.6597 0.7500 0.9344 6 7 6 7 6 7 6 7 6 7 0.7891 0.4356 0.4330 0.2573 6 7 6 7 . (44) 6 7 6 7 6 7 0.6124 0.6124 0.5000 0.0017 6 7 6 7 4 5 0 0 0 1 All joint angles were solved with the algorithm, as shown in Table 15. Table 15. Four inversely solved joint angle sets. No. / / / / / / 2 3 5 6 1 4 1 150.0000 60.0000 60.0000 95.1093 100.7234 129.3859 2 150.0000 60.0000 60.0000 9.6646 100.7234 114.6120 3 90.0000 60.0000 60.0000 45.0000 45.0000 45.0000 4 90.0000 60.0000 60.0000 84.7298 45.0000 95.2702 It is easy to verify that the four sets of joint angles are correct. In this group of experiments, the test subjects were two uncommon robots constructed by basic questions. However, according to the proposed theory, these two robots had a closed inverse solution. In the experiment, the inverse solution of both robots was solved correctly. In addition, for the ﬁrst robot, its conﬁguration remains unchanged, but the link parameters were greatly modiﬁed, the correct Appl. Sci. 2019, 9, 4365 20 of 36 result was obtained in the end. It could be seen that the algorithm had good versatility. As long as a given robot can be described by three sub-problems, its closed-from solution can be obtained whether it is in low degree of freedom or six degrees of freedom. At the same time, for the robot with the same conﬁguration, the change of the link parameters does not aect the inverse kinematics solution. 5.3. Veriﬁcation Experiment for the Algorithm Continuity This experiment would use the common Puma560 as the subject. In the experiment, the robot should move according to the speciﬁed trajectory. We needed to observe whether the joint angle after the inverse solution was continuous when solving the continuous space trajectory. This is important in the actual movement of robot. If there is a jump or discontinuity in the joint space curve, it will cause great impact to the motor, and the curve of the end also does not move according to the speciﬁed trajectory. The DH parameters of the Puma560 robot are shown in Table 16. Table 16. The DH parameters of the Puma560 robot. n / d/mm a/mm / 1 0 0 /2 2 0 43.18 0 3 150.03 20.3 /2 4 43.18 0 /2 5 0 0 /2 6 0 0 0 The quantity of the vertical joints was judged following the input of the algorithm into the DH model of the Puma560 robot. Subsequently, the algorithm entered the ﬁrst type of sub-problem. Then, joint decoupling was performed for J and J ?J k J ?J ?J . Based on basic problem 3.4.1, 6 1 2 3 4,a 5,a,d the angle value of was obtained. In addition, after was obtained, T was obtained using to 1 1 the forward kinematics formula. After it was simpliﬁed, J k J ?J ?J ?J formed a new robot 2 3 4,a 6 5,a,d model and was substituted into the algorithm. Similarly, J k J ?J ?J was solved using basic 2 3 4,a 5,a,d problem 3.2.2 and and were subsequently obtained. After the reduction in the DH parameters 2 3 and the transformational matrices, T and the robot model J ?J ?J were substituted into the 4,a 5,a,d 6 algorithm to solve the rational component in the second type of sub-problem. All joint angles were solved at that time. 2 3 25.5667 6 7 6 7 6 7 6 7 6 7 0.0624 6 7 6 7 6 7 6 7 6 7 3.0736 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end execution posture was: 6 7 6 7 25.5975 6 7 6 7 6 7 6 7 6 7 87.2840 6 7 6 7 4 5 1.3005 2 3 0 0 1 0.4521 6 7 6 7 6 7 6 7 6 7 0 1 0 0.0499 6 7 6 7 . (45) 6 7 6 7 6 7 1 0 0 0.4318 6 7 6 7 4 5 0 0 0 1 All joint angles were computed with the algorithm, as shown in Table 17. Here we also used the kinematic inverse function in the Robotics Toolbox, and could also ﬁnd eight groups of solutions, as shown in Table 18. It could be seen that part of the joint angles have exceeded h i the scope of . The reason is that there was no uniform inverse trigonometric function selected in the Robotics Toolbox. In actual use, for example, the Beckho-C6920 controller and the Kollmorgen RGM motor, both use real numbers to describe the motion of the motor rather than a positive real number. Therefore, the method proposed in this paper had certain advantages from this point of view. Appl. Sci. 2019, 9, 4365 21 of 36 Table 17. Eight inversely solved joint angle sets. No. / / / / / / 1 2 3 4 5 6 1 167.0434 89.6199 3.0711 78.4733 166.7756 101.8339 2 167.0434 89.6199 3.0711 101.5267 166.7756 78.1661 3 167.0434 179.9370 177.6878 167.0362 92.3124 0.5341 4 167.0434 179.9370 177.6878 12.9638 92.3124 179.4659 5 25.5654 0.0630 3.0711 25.5998 87.2844 1.3006 6 25.5654 0.0630 3.0711 154.4002 87.2844 178.6994 7 25.5654 90.3801 177.6878 84.3875 154.2989 83.7756 8 25.5654 90.3801 177.6878 95.6125 154.2989 96.2244 Appl. Sci. 2019, 9, x FOR PEER REVIEW 22 of 36 Table 18. Eight inversely solved joint angle sets by the Robotics Toolbox. Table 18. Eight inversely solved joint angle sets by the Robotics Toolbox. No. / / / / / / 1 2 3 4 5 6 1 167.0434 89.6199 3.0711 78.4733 166.7756 101.8339 No. 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝟏 𝟐 𝟑 𝟒 𝟓 𝟔 2 167.0434 89.6199 3.0711 101.5267 166.7756 78.1661 1 167.0434 89.6199 3.0711 -78.4733 166.7756 101.8339 3 167.0434 180.0624 182.3097 167.0320 92.3124 0.5322 2 167.0434 89.6199 3.0711 101.5267 -166.7756 -78.1661 4 167.0434 180.0624 182.3097 12.9638 92.3124 179.4659 3 167.0434 180.0624 182.3097 -167.0320 92.3124 0.5322 5 25.5654 90.3801 182.3097 84.3875 154.2989 83.7756 6 25.5654 90.3801 182.3097 95.6086 154.2989 96.2197 4 167.0434 180.0624 182.3097 12.9638 -92.3124 -179.4659 7 25.5654 0.0624 3.0736 25.5975 87.2840 1.3005 5 25.5654 90.3801 182.3097 -84.3875 154.2989 -83.7756 8 25.5654 0.0624 3.0736 154.4025 87.2840 178.6995 6 25.5654 90.3801 182.3097 95.6086 -154.2989 96.2197 7 25.5654 -0.0624 3.0736 -25.5975 87.2840 1.3005 The robot was instructed to move as per the Equation (51) spiral line with a step size of 0.01: 8 25.5654 0.0624 3.0736 154.4025 -87.2840 -178.6995 > x = 0.3t The robot was instructed to mov >e as per the Equation (51) spiral line with a step size of 0.01: y = 0.2cos(2t) 0.2 , t 2 [0, 1]. (46) 𝑥 = > 0.3𝑡 (46) 𝑦= 0.2 𝑐𝑜𝑠 (2𝜋𝑡z = ) −0 0.2sin .2 (2t) ,𝑡 ∈ [0,1]. 𝑧= 0.2𝑖𝑛𝑠(2𝜋𝑡) According to formula (40), with the ﬁfth set of solutions selected. According to the joint angle, According to formula (40), with the fifth set of solutions selected. According to the joint angle, the end position was recomputed with the motion trajectory shown in Figure 3. The joint angle was the end position was recomputed with the motion trajectory shown in Figure 3. The joint angle was inversely solved as per the end trajectory to obtain the continuous joint position, as shown in Figure 4. inversely solved as per the end trajectory to obtain the continuous joint position, as shown in Figure Figure 3. The motion trajectory of the Puma560 robot. Figure 3. The motion trajectory of the Puma560 robot. As shown in Table 17, the algorithm could solve for multiple sets of joint angles, and it could be conﬁrmed via positive kinematics that all the joint angles were correct. In addition, in the planning experiment of the Cartesian space trajectory, the joint angle inversely solved according to the target trajectory is shown in Figure 4 and its curve changed continuously without jumps. In Figure 3, the end Figure 4. The joint trajectory after the inverse kinematics solution. Appl. Sci. 2019, 9, x FOR PEER REVIEW 22 of 36 Table 18. Eight inversely solved joint angle sets by the Robotics Toolbox. No. 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝟏 𝟐 𝟑 𝟒 𝟓 𝟔 1 167.0434 89.6199 3.0711 -78.4733 166.7756 101.8339 2 167.0434 89.6199 3.0711 101.5267 -166.7756 -78.1661 3 167.0434 180.0624 182.3097 -167.0320 92.3124 0.5322 4 167.0434 180.0624 182.3097 12.9638 -92.3124 -179.4659 5 25.5654 90.3801 182.3097 -84.3875 154.2989 -83.7756 6 25.5654 90.3801 182.3097 95.6086 -154.2989 96.2197 7 25.5654 -0.0624 3.0736 -25.5975 87.2840 1.3005 8 25.5654 0.0624 3.0736 154.4025 -87.2840 -178.6995 The robot was instructed to move as per the Equation (51) spiral line with a step size of 0.01: 𝑥 = 0.3𝑡 (46) 𝑦= 0.2 𝑐𝑜𝑠 (2𝜋𝑡 ) −0.2 [ ] ,𝑡 ∈ 0,1 . 𝑧= 0.2𝑖𝑛𝑠(2𝜋𝑡) According to formula (40), with the fifth set of solutions selected. According to the joint angle, the end position was recomputed with the motion trajectory shown in Figure 3. The joint angle was inversely solved as per the end trajectory to obtain the continuous joint position, as shown in Figure Appl. Sci. 2019, 9, 4365 22 of 36 trajectory was recalculated from the obtained joint angle and was consistent with the planned trajectory. Figure 3. The motion trajectory of the Puma560 robot. This proved that the algorithm could correctly solve the inverse kinematics of the spatial trajectory. Appl. Sci. 2019, 9, x FOR PEER REVIEW 23 of 36 As shown in Table 17, the algorithm could solve for multiple sets of joint angles, and it could be confirmed via positive kinematics that all the joint angles were correct. In addition, in the planning experiment of the Cartesian space trajectory, the joint angle inversely solved according to the target trajectory is shown in Figure 4 and its curve changed continuously without jumps. In Figure 3, the end trajectory was recalculated from the obtained joint angle and was consistent with the planned trajectory. This proved that the algorithm could correctly solve the inverse kinematics of the spatial trajectory. 5.4. Real-Time Verification Experiment of the Algorithm Figure 4. The joint trajectory after the inverse kinematics solution. Figure 4. The joint trajectory after the inverse kinematics solution. In the industry, the robot controller sends position information to the motor driver in a certain 5.4. Real-Time Veriﬁcation Experiment of the Algorithm cycle, the shorter the cycle, the better the real-time performance. For some occasions with high In the industry, the robot controller sends position information to the motor driver in a certain precision, if the cycle is too long, a big error will occur in the outline of the end. In addition, if the cycle, the shorter the cycle, the better the real-time performance. For some occasions with high precision, desired end trajectory is constantly changing, the controller is also required to be able to react and if the cycle is too long, a big error will occur in the outline of the end. In addition, if the desired end plan quickly during the cycle. Therefore, real-time performance is an important performance trajectory is constantly changing, the controller is also required to be able to react and plan quickly indicator in the robot controlling field. In order to improve the closed-loop period of the system, in during the cycle. Therefore, real-time performance is an important performance indicator in the robot addition to the purchase of more powerful hardware devices, the more important factor is the controlling field. In order to improve the closed-loop period of the system, in addition to the purchase operation speed of algorithm. Therefore, this paper designed a verification experiment whose real- of more powerful hardware devices, the more important factor is the operation speed of algorithm. time performance was closed to prove that the algorithm proposed in this paper was not only Therefore, this paper designed a verification experiment whose real-time performance was closed to accurate but also efficient and fast. prove that the algorithm proposed in this paper was not only accurate but also efficient and fast. The six-DOF KingKong robot built with a Beckhoff-C6920 controller and a Kollmorgen RGM The six-DOF KingKong robot built with a Beckho-C6920 controller and a Kollmorgen RGM motor was used in the experiment testing the correctness and real-time performance of the algorithm. motor was used in the experiment testing the correctness and real-time performance of the algorithm. The DH parameters of the robot are listed in Table 3. The C6920 controller in Figure 5. The C6920 The DH parameters of the robot are listed in Table 3. The C6920 controller in Figure 5. The C6920 controller used a 32-bit Windows 7 operating system and was equipped with an Intel Core i5 controller used a 32-bit Windows 7 operating system and was equipped with an Intel Core i5 processor. processor. Figure Figure 5. 5. The C6920 The C6920 controller. controller. Figure 6 displays the control framework for the entire system. When the controller received the Figure 6 displays the control framework for the entire system. When the controller received the order of motion, the critical parameters of the locus equation were computed according to the order. order of motion, the critical parameters of the locus equation were computed according to the order. Subsequently, the position point at the next time point was planned in the Cartesian spatial planner. Based on the position points in Cartesian space, the inverse kinematics solving method proposed here was employed to obtain the position to be reached by each motor at the next time point. The closed-loop circle of the motor controller was 2 ms. The controller sent the motion order to the RGM motor via CanOpen. CanOpen is a high-level communication protocol based on the Controller Area Network. It is often used in embedded systems and is a type of field bus commonly used in industrial control. Appl. Sci. 2019, 9, 4365 23 of 36 Subsequently, the position point at the next time point was planned in the Cartesian spatial planner. Based on the position points in Cartesian space, the inverse kinematics solving method proposed here was employed to obtain the position to be reached by each motor at the next time point. The closed-loop Appl. Sci. 2019, 9, x FOR PEER REVIEW 24 of 36 circle of the motor controller was 2 ms. The controller sent the motion order to the RGM motor via CanOpen. CanOpen is a high-level communication protocol based on the Controller Area Network. It is often used in embedded systems and is a type of ﬁeld bus commonly used in industrial control. Appl. Sci. 2019, 9, x FOR PEER REVIEW 24 of 36 Cartesian space planner M M M RGM14 RGM14 RGM14 Cartesian space planner Ikine Ikine M M M RGM14 RGM14 RGM14 CanOpen M M M Ikine Ikine Motion RGM20 RGM25 RGM25 CanOpen M M Beckhoff-C6920 Motion RGM20 RGM25 RGM25 Figure 6. Control structure of the system. Beckhoff-C6920 The number of vertical joints was judged according to the KingKong DH model. Then, the Figure 6. Control structure of the system. Figure 6. Control structure of the system. algorithm assessed the first type of sub-problem. After joint decoupling was performed for 𝐽 , 𝐽 ⊥ 𝐽 ∥𝐽 The ∥𝐽 number ⊥𝐽 wa of vertical s able to joints solve was for judged 𝜃 uaccor sing b ding asicto pthe robKingKong lem 3.2.1. A DH ftmodel. er 𝜃 wa Then, s ob the tain algorithm ed, 𝑇 and The number of vertical joints was judged according to the KingKong DH model. Then, the assessed the ﬁrst type of sub-problem. After joint decoupling was performed for J , J ?J k J k J ?J 𝐽 ∥𝐽 ∥𝐽 ⊥𝐽 ⊥𝐽 were obtained using the forward kinematics formula. At t 6 he time, t 2 3 he 5, robot a 1 4 algorithm assessed the first type of sub-problem. After joint decoupling was performed for 𝐽 , 𝐽 ⊥ was able to solve for using basic problem 3.2.1. After was obtained, T and J k J k J ?J ?J belonged to the second type of sub-pr 1 oblem and we could d 1 irectly solve for 𝜃 2+𝜃 3 +𝜃 4 ,𝜃 5,a and 6 𝜃 . 𝐽 ∥𝐽 ∥𝐽 ⊥𝐽 was able to solve for 𝜃 using basic problem 3.2.1. After 𝜃 was obtained, 𝑇 and were obtained using the forward kinematics formula. At the time, the robot belonged to the second Subsequently, basic problem 3.4.2 was used to solve for 𝜃 and 𝜃 +𝜃 and to obtain 𝜃 and 𝜃 . 𝐽 ∥𝐽 ∥𝐽 ⊥𝐽 ⊥𝐽 were obtained using the forward kinematics formula. At the time, the robot type of sub-problem and we could directly solve for + + , and . Subsequently, basic 2 3 4 5 6 In this experiment, the robot would be commanded to move according to the specified spatial belonged to the second type of sub-problem and we could directly solve for 𝜃 +𝜃 +𝜃 ,𝜃 and 𝜃 . problem 3.4.2 was used to solve for and + and to obtain and . 2 2 3 3 trajectory in the real environment, and the acceleration process of the motor must be considered. A Subsequently, basic problem 3.4.2 was used to solve for 𝜃 and 𝜃 +𝜃 and to obtain 𝜃 and 𝜃 . In this experiment, the robot would be commanded to move according to the speciﬁed spatial common acceleration planning method is a seven-segment S curve. This method uses the square In this experiment, the robot would be commanded to move according to the specified spatial trajectory in the real environment, and the acceleration process of the motor must be considered. wave Jerk curve as the input of the system, and the acceleration, velocity, and position quantities trajectory in the real environment, and the acceleration process of the motor must be considered. A A common acceleration planning method is a seven-segment S curve. This method uses the square obtained through the integrating of time, as is shown Figure 7. common acceleration planning method is a seven-segment S curve. This method uses the square wave Jerk curve as the input of the system, and the acceleration, velocity, and position quantities wave Jerk curve as the input of the system, and the acceleration, velocity, and position quantities obtained through the integrating of time, as is shown Figure 7. obtained through the integrating of time, as is shown Figure 7. (a) (a) (b) (b) (c) (c) (d) (d) Figure 7. (a) Displacement of the seven-segment S curve over time. (b) Velocity of the seven-segment S Figure 7. (a) Displacement of the seven-segment S curve over time. (b) Velocity of the seven-segment curve over time. (c) Acceleration of the seven-segment S curve over time. (d) Jerk of seven-segment S S curve over time. (c) Acceleration of the seven-segment S curve over time. (d) Jerk of seven-segment curve over time. S curve over time. Figure 7. (a) Displacement of the seven-segment S curve over time. (b) Velocity of the seven-segment S curve over time. (c) Acceleration of the seven-segment S curve over time. (d) Jerk of seven-segment It can be seen from the figure that the acceleration curve of the seven-segment S-curve S curve over time. constructed by square wave Jerk input is a linear piecewise function. Although it is continuous, but there are a finite number of non-conductible breakpoints. If the acceleration can be made to have It can be seen from the figure that the acceleration curve of the seven-segment S-curve higher order differentiable properties, the robot movement can be smoother. constructed by square wave Jerk input is a linear piecewise function. Although it is continuous, but In the Descartes spatial planner of the controller, the 15-segment S-curve spatial trajectory there are a finite number of non-conductible breakpoints. If the acceleration can be made to have planning method was used. When the objective position, S, and the maximum values of the curve, higher order differentiable properties, the robot movement can be smoother. 𝑆 ,𝑆 ,and 𝑆 , are given, this method can provide a smooth curve and the speed utilization rate can In the Descartes spatial planner of the controller, the 15-segment S-curve spatial trajectory approximate the optimal solution. In 15-segment S planning, the jerk input uses the sine piecewise planning method was used. When the objective position, S, and the maximum values of the curve, 𝑆 ,𝑆 ,and 𝑆 , are given, this method can provide a smooth curve and the speed utilization rate can approximate the optimal solution. In 15-segment S planning, the jerk input uses the sine piecewise Appl. Sci. 2019, 9, x FOR PEER REVIEW 25 of 36 function Equation (52) to replace the jerk square wave input in the traditional seven-segment S curve. A schematic diagram for the 15-segment S curve is shown in Figure 8. Appl. Sci. 2019, 9, 4365 24 of 36 Compared with the traditional seven-segment S curve, the 15-segment curve proposed in this paper was constructed by sinusoidal function. We know that sinusoidal functions have infinite It can be seen from the ﬁgure that the acceleration curve of the seven-segment S-curve constructed differentiable properties. Therefore, the acceleration curve of the 15-segment S curve also has infinite by square wave Jerk input is a linear piecewise function. Although it is continuous, but there are a differentiable properties. The curve obtained by this planning method is smoother. ﬁnite number of non-conductible breakpoints. If the acceleration can be made to have higher order dierentiable properties, the robot movement can be smoother. 𝑠𝑖𝑛 𝑛𝑡 𝑥 ∈ [0, ] In the Descartes ⎪ spatial planner of the controller, the 15-segment S-curve spatial trajectory planning . .. ... (47) ℎ (𝑡 ) = 1𝑥∈[ , +𝑚] . method was used. When the objective position, S, and the maximum values of the curve, S, S, and , 𝑐𝑜𝑠 𝑛(𝑡 − 𝑚) 𝑥 ∈ [ +𝑚, +𝑚] are given, this method can provide a smooth curve and the speed utilization rate can approximate the optimal solution. In 15-segment S planning, the jerk input uses the sine piecewise function Equation (52) In Equation (47), 𝑛 is an adjustable parameter to adjust the rising speed of the jerk curve and 𝑚 to replace the jerk square wave input in the traditional seven-segment S curve. A schematic diagram for the 15-segment S curve is shown in Figure 8. depends on the maximum values of 𝑆 and 𝑆 . (a) (b) (c) (d) Figure 8. (a) Displacement of the 15-segment S curve over time. (b) Velocity of the 15-segment S curve Figure 8. (a) Displacement of the 15-segment S curve over time. (b) Velocity of the 15-segment S curve over time. (c) Acceleration of the 15-segment S curve over time. (d) Jerk of 15-segment S curve over time. over time. (c) Acceleration of the 15-segment S curve over time. (d) Jerk of 15-segment S curve over Compared with the traditional seven-segment S curve, the 15-segment curve proposed in this time. paper was constructed by sinusoidal function. We know that sinusoidal functions have inﬁnite dierentiable properties. Therefore, the acceleration curve of the 15-segment S curve also has inﬁnite 0° ⎡ ⎤ −60° dierentiable properties. The curve obtained by this planning method is smoother. ⎢ ⎥ 120° 8 h ⎢i ⎥ The initial position of the KingKong robot was Θ= , as shown in Figure 9. > 1 > −135° sin nt x 2 0, ⎢ ⎥ 2 n > h i ⎢ −45° ⎥ 1 1 h (t) = 1 x 2 , + m . (47) n > n n > ⎣ ⎦ h i −45° 1 2 cos n(t m) x 2 + m, + m 2 n n In Equation (47), n is an adjustable parameter to adjust the rising speed of the jerk curve and m .. ... depends on the maximum values of S and S . 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 The initial position of the KingKong robot was Q = 6 7, as shown in Figure 9. 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 (a) ( b) Figure 9. (a) Robot in its initial position from the front. (b) Robot in its initial position from the side. At that time, based on the forward kinematics computation, the end posture was: Appl. Sci. 2019, 9, x FOR PEER REVIEW 25 of 36 function Equation (52) to replace the jerk square wave input in the traditional seven-segment S curve. A schematic diagram for the 15-segment S curve is shown in Figure 8. Compared with the traditional seven-segment S curve, the 15-segment curve proposed in this paper was constructed by sinusoidal function. We know that sinusoidal functions have infinite differentiable properties. Therefore, the acceleration curve of the 15-segment S curve also has infinite differentiable properties. The curve obtained by this planning method is smoother. 𝑠𝑖𝑛 𝑛𝑡 𝑥 ∈ [0, ] (47) ℎ (𝑡 ) = 1𝑥∈[ , +𝑚] . 𝑐𝑜𝑠 𝑛(𝑡 − 𝑚) 𝑥 ∈ [ +𝑚, +𝑚] In Equation (47), 𝑛 is an adjustable parameter to adjust the rising speed of the jerk curve and 𝑚 depends on the maximum values of 𝑆 and 𝑆 . (a) (b) (c) (d) Figure 8. (a) Displacement of the 15-segment S curve over time. (b) Velocity of the 15-segment S curve over time. (c) Acceleration of the 15-segment S curve over time. (d) Jerk of 15-segment S curve over time. 0° ⎡ ⎤ −60° ⎢ ⎥ 120° ⎢ ⎥ The initial position of the KingKong robot was Θ= , as shown in Figure 9. −135° ⎢ ⎥ ⎢ ⎥ Appl. Sci. 2019, 9, 4365 −45° 25 of 36 ⎣ ⎦ −45° (a) (b) Figure 9. (a) Robot in its initial position from the front. (b) Robot in its initial position from the side. Figure 9. (a) Robot in its initial position from the front. (b) Robot in its initial position from the side. At that time, based on the forward kinematics computation, the end posture was: At that time, based on the forward kinematics computation, the end posture was: 2 3 0.5536 0.8124 0.1830 0.4961 6 7 6 7 6 7 6 7 6 7 0.5000 0.5000 0.7071 0.1330 6 7 6 7 6 7. (48) 6 7 6 7 0.6660 0.3000 0.6830 0.0895 6 7 6 7 4 5 0 0 0 1 The algorithm was used to obtain the inverse solution and eight sets of joint angles, as shown in Table 19. According to formula (40) the fourth set of solutions was selected and applied in the motion control logic. Table 19. Eight sets of reversely solved joint angles. No. / / / / / / 1 2 3 4 5 6 1 0 38.3143 80.0936 146.7793 45.0000 135.0000 2 0 35.7525 80.0936 60.6588 45.0000 135.0000 3 0 47.6172 120.0000 2.6172 45.0000 45.0000 4 0 60.0000 120.0000 135.0000 45.0000 45.0000 5 159.2347 125.5572 120.8932 16.9043 136.5570 15.1304 6 159.2347 126.1566 120.8932 150.4045 136.5570 15.1304 7 159.2347 140.7508 79.3110 136.7071 136.5570 164.8696 8 159.2347 145.8820 79.3110 51.4522 136.5570 164.8696 The spatial planning was as follows. The experiment used a spiral line as the expected end trajectory. The parameter equation of the end trajectory spiral line is: > x = 0.15 cos(2t) 0.15 > y = 0.15 sin(t) , t 2 [0, 5] . (49) z = 0.05t First, according to Equation (50), the arc length of the spiral line is: ! ! ! 2 2 2 dy dx dz l = + + dt = 2.3694 m. (50) dt dt dt . .. ... Given S = 0.125 m/s, S = 0.025 m/s, = 0.01 m/s, and n = 1, the 15-segment S curve max max S max was used to plan the end arc length. The total time consumed was 28.1821 s. The variation curve at each stage of the trajectory is shown in Figure 10. Appl. Sci. 2019, 9, x FOR PEER REVIEW 26 of 36 −0.5536 0.8124 0.1830 −0.4961 0.5000 0.5000 −0.7071 −0.1330 (48) −0.6660 −0.3000 −0.6830 0.0895 The algorithm was used to obtain the inverse solution and eight sets of joint angles, as shown in Table 19. According to formula (40) the fourth set of solutions was selected and applied in the motion control logic. Table 19. Eight sets of reversely solved joint angles. No. 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝟏 𝟐 𝟑 𝟒 𝟓 𝟔 1 0 38.3143 -80.0936 146.7793 45.0000 135.0000 2 0 -35.7525 80.0936 60.6588 45.0000 135.0000 3 0 47.6172 -120.0000 -2.6172 -45.0000 -45.0000 4 0 -60.0000 120.0000 -135.0000 -45.0000 -45.0000 5 -159.2347 -125.5572 -120.8932 -16.9043 136.5570 -15.1304 6 -159.2347 126.1566 120.8932 -150.4045 136.5570 -15.1304 7 -159.2347 -140.7508 -79.3110 136.7071 -136.5570 164.8696 8 -159.2347 145.8820 79.3110 51.4522 -136.5570 164.8696 The spatial planning was as follows. The experiment used a spiral line as the expected end trajectory. The parameter equation of the end trajectory spiral line is: ( ) 𝑥 = 0.15 cos 2𝜋𝑡 − 0.15 (49) , 𝑡 ∈ [0,5]. 𝑦 = 0.15 sin (𝜋𝑡 ) 𝑧 = 0.05𝑡 First, according to Equation (50), the arc length of the spiral line is: 𝑑𝑥 𝑑𝑦 𝑑𝑧 (50) ℓ= ( ) +( ) +( ) = 2.3694 m. 𝑑𝑡 𝑑𝑡 𝑑𝑡 Given 𝑆 = 0.125 m/s， 𝑆 = 0.025 m/s， 𝑆 =0.01 m/s, and 𝑛= 1 , the 15-segment S curve was used to plan the end arc length. The total time consumed was 28.1821 s. The variation curve at each stage of the trajectory is shown in Figure 10. Appl. Sci. 2019, 9, 4365 26 of 36 Appl. Sci. 2019, 9, x FOR PEER REVIEW 27 of 36 Figure 10. (a) Displacement of the spatial trajectory over time. (b) Velocity of the spatial trajectory over time. (c) Acceleration of the spatial trajectory over time. (d) Jerk of the spatial trajectory over (a) (b) (c) (d) time. Figure 10. (a) Displacement of the spatial trajectory over time. (b) Velocity of the spatial trajectory over The arc length obtained according to Equation (50) was used to rewrite the parametric equation: time. (c) Acceleration of the spatial trajectory over time. (d) Jerk of the spatial trajectory over time. ⎧𝑥 = 0.15 ( ) − 0.15 The arc length obtained according to Equation (50) was used to rewrite the parametric equation: ⎪ 0.1581 8 ,𝑙 ∈ [0, ℓ]. (51) > x = 0.15 cos 0.15 ⎨ 𝑦 = 0.15 𝑠𝑖𝑛( ) > 0.1581 < 0.1581 , l 2 [0,l] . (51) > y = 0.15 sin ⎩ > 0.1581 > 𝑧 = 0.1581 𝑙 z = 0.1581 l In summary, the position curve in Cartesian space and its various derivatives could be obtained, In summary, the position curve in Cartesian space and its various derivatives could be obtained, together with the planned position curves of each joint and their various derivatives, as shown in together with the planned position curves of each joint and their various derivatives, as shown in Figures 11–16. Figures 11–16. (a) (b) (c) (d) Figure 11. (a) Displacement of axis I over time. (b) Velocity of axis I over time. (c) Acceleration of axis I Figure 11. (a) Displacement of axis I over time. (b) Velocity of axis I over time. (c) Acceleration of axis over time. (d) Jerk of axis I over time. I over time. (d) Jerk of axis I over time. (a) (b) (c) (d) 𝑐𝑜𝑠 𝑑𝑡 Appl. Sci. 2019, 9, x FOR PEER REVIEW 27 of 36 Figure 10. (a) Displacement of the spatial trajectory over time. (b) Velocity of the spatial trajectory over time. (c) Acceleration of the spatial trajectory over time. (d) Jerk of the spatial trajectory over time. The arc length obtained according to Equation (50) was used to rewrite the parametric equation: 𝑥 = 0.15 ( ) − 0.15 0.1581 (51) 𝑙 ,𝑙 ∈ [0, ℓ]. ⎨ 𝑦 = 0.15 𝑠𝑖𝑛( ) 0.1581 𝑧 = 0.1581 𝑙 In summary, the position curve in Cartesian space and its various derivatives could be obtained, together with the planned position curves of each joint and their various derivatives, as shown in Figures 11–16. (a) (b) (c) (d) Figure 11. (a) Displacement of axis I over time. (b) Velocity of axis I over time. (c) Acceleration of axis I over time. (d) Jerk of axis I over time. Appl. Sci. 2019, 9, 4365 27 of 36 Appl. Sci. 2019, 9, x FOR PEER REVIEW 28 of 36 Figure 12. (a) Displacement of axis II over time. (b) Velocity of axis II over time. (c) Acceleration of (a) (b) (c) (d) axis II over time. (d) Jerk of axis II over time. Figure 12. (a) Displacement of axis II over time. (b) Velocity of axis II over time. (c) Acceleration of axis II over time. (d) Jerk of axis II over time. (a) (b) (c) (d) Figure 13. (a) Displacement of axis III over time. (b) Velocity of axis III over time. (c) Acceleration of Figure 13. (a) Displacement of axis III over time. (b) Velocity of axis III over time. (c) Acceleration of axis III over time. (d) Jerk of axis III over time. axis III over time. (d) Jerk of axis III over time. (a) (b) (c) (d) Figure 14. (a) Displacement of axis IV over time. (b) Velocity of axis IV over time. (c) Acceleration of axis IV over time. (d) Jerk of axis IV over time. 𝑐𝑜𝑠 Appl. Sci. 2019, 9, x FOR PEER REVIEW 28 of 36 Figure 12. (a) Displacement of axis II over time. (b) Velocity of axis II over time. (c) Acceleration of axis II over time. (d) Jerk of axis II over time. (a) (b) (c) (d) Figure 13. (a) Displacement of axis III over time. (b) Velocity of axis III over time. (c) Acceleration of axis III over time. (d) Jerk of axis III over time. Appl. Sci. 2019, 9, 4365 28 of 36 Appl. Sci. 2019, 9, x FOR P (a)EER REVIEW (b) (c) (d) 29 of 36 Figure 14. (a) Displacement of axis IV over time. (b) Velocity of axis IV over time. (c) Acceleration of Figure 14. (a) Displacement of axis IV over time. (b) Velocity of axis IV over time. (c) Acceleration of axis IV over time. (d) Jerk of axis IV over time. axis IV over time. (d) Jerk of axis IV over time. (a) (b) (c) (d) Figure 15. Figure (a 15. ) Displacem (a) Displacement ent of ax ofis axis V over V over time. time. (b (b ) Veloc ) Velocity ity of of ax axis isV V ov overetime. r time (. c ( ) c Acceleration ) Acceleration of of axis V over time. (d) Jerk of axis V over time. axis V over time. (d) Jerk of axis V over time. (a) (b) (c) (d) Figure 16. (a) Displacement of axis VI over time. (b) Velocity of axis VI over time. (c) Acceleration of axis VI over time. (d) Jerk of axis VI over time. Figure 17 shows the position after the robot has finished its run. The curve of the actual motion of the robot in Cartesian space was calculated from the forward kinematics and is shown together with the planned spatial curve in Figure 18. After the run was completed, the actual motion curve of each joint was acquired from the controller, as shown in Figure 19. Appl. Sci. 2019, 9, x FOR PEER REVIEW 29 of 36 (a) (b) (c) (d) Figure 15. (a) Displacement of axis V over time. (b) Velocity of axis V over time. (c) Acceleration of axis V over time. (d) Jerk of axis V over time. Appl. Sci. 2019, 9, 4365 29 of 36 (a) (b) (c) (d) Figure 16. (a) Displacement of axis VI over time. (b) Velocity of axis VI over time. (c) Acceleration of Figure 16. (a) Displacement of axis VI over time. (b) Velocity of axis VI over time. (c) Acceleration of axis VI over time. (d) Jerk of axis VI over time. axis VI over time. (d) Jerk of axis VI over time. Figure 17 shows the position after the robot has ﬁnished its run. The curve of the actual motion of Figure 17 shows the position after the robot has finished its run. The curve of the actual motion the robot in Cartesian space was calculated from the forward kinematics and is shown together with of the robot in Cartesian space was calculated from the forward kinematics and is shown together the planned spatial curve in Figure 18. After the run was completed, the actual motion curve of each Appl. Appl. Sci. Sci. 2019 2019 , 9 , ,9 x FO , x FO R P R P EER EER RE RE VIEW VIEW 30 of 30 of 36 36 with the planned spatial curve in Figure 18. After the run was completed, the actual motion curve of joint was acquired from the controller, as shown in Figure 19. each joint was acquired from the controller, as shown in Figure 19. (a () ( a) ( bb ) ) Figure 17. Figure Figure 17. 17. (a( () a aPosture of the ) ) Posture of the Posture of the ro ro rbot after obot bot after after completion of completion completion of of its run from the front. its run from the front. its run from the front ( . b ( (b ) b Posture of ) ) Posture of Posture of the robot the the robot robot after completio after completion n of it of its s run fro run from m the side. the side. after completion of its run from the side. Figure 18. Robot end trajectory. Figure 18. Robot end trajectory. Figure 18. Robot end trajectory. Figure 19. Figure 19. The joint The joint trajectory after trajectory after the the in iver nver se ki se ki nemati nemati cs c s s s olo u lti uti on. on. In the fourth experiment, the Beckhoff controller, which is commonly used in the industry, was In the fourth experiment, the Beckhoff controller, which is commonly used in the industry, was used used and and the the general algor general algor ithm w ithm w as as implemented implemented in in th th e con e con troller. T troller. T hh e robo e robo t con t con trto rller oller sends m sends m otion otion commands to the drive at a certain period. This requires the controller to complete the kinematic commands to the drive at a certain period. This requires the controller to complete the kinematic inverse inverse sol sol uu titi on on with with in a s in a s pp ecif ecif ied amo ied amo uu nt nt of o time f time to to ensu ensu re re the co the co ntin ntin ui ui ty. ty. A A sh sh orter p orter p eriod eriod r r esu esu ltlt s s in a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sufficient in a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sufficient to m to m ee ete tthe the g g enera enera l l accur accur acy re acy re qu qu irem irem en en ts. ts. For For ex ex am am ple, in Re ple, in Re f. [5 f. [5 ] the ] the med med icia cla lrobot robot can can ac ac cept cept a a closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve a closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve a Appl. Sci. 2019, 9, x FOR PEER REVIEW 30 of 36 (a) (b) Figure 17. (a) Posture of the robot after completion of its run from the front. (b) Posture of the robot after completion of its run from the side. Appl. Sci. 2019, 9, 4365 30 of 36 Figure 18. Robot end trajectory. Figure 19. The joint trajectory after the inverse kinematics solution. Figure 19. The joint trajectory after the inverse kinematics solution. In the fourth experiment, the Beckho controller, which is commonly used in the industry, was In the fourth experiment, the Beckhoff controller, which is commonly used in the industry, was used and the general algorithm was implemented in the controller. The robot controller sends motion used and the general algorithm was implemented in the controller. The robot controller sends motion commands to the drive at a certain period. This requires the controller to complete the kinematic commands to the drive at a certain period. This requires the controller to complete the kinematic inverse solution within a speciﬁed amount of time to ensure the continuity. A shorter period results in inverse solution within a specified amount of time to ensure the continuity. A shorter period results a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sucient in a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sufficient to meet the general accuracy requirements. For example, in Ref. [5] the medical robot can accept a to meet the general accuracy requirements. For example, in Ref. [5] the medical robot can accept a closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve a a closed-loop period higher than industrial demand in equipment commonly used in the industry. The fast closed-loop period means that the algorithm has higher computational eciency. Since our algorithm could decompose complex inverse motion problems into several sub-problems and solve them one by one, all joint angles could be solved accurately and quickly. The ﬁnal experimental results show that the system could complete the inverse solution of the kinematics within the speciﬁed period. This allows the algorithm to run stably in situations with real-time requirements. Figure 8 shows the end curve planned to use 15 S-curve segments. The apparent rise and fall process in the jerk curve could be seen. This allows the entire curve to have a smoother transition at start–stop. Based on Equation (46) and the plan in Figure 10, the angular curve of each joint could be obtained, as shown in Figures 11–16. It can be seen from Figures 11–16 that all joint motion curves, as well as the various derivative curves could be ensured to start from 0 during the start-up phase smoothly. The continuous and smooth was kept in the whole moving process. Moreover, the curve was smoothly reduced to zero during the stop phase. Especially the fourth derivative had kept the continuous and smooth ﬂuctuations, which was the eect that could not be realized through traditional planning method. This paper introduced the joint curve obtained through using the traditional seven-segment planning there to facilitate the comparison, as shown in Figures 20–25. It can be clearly seen that the Jerk curve of each joint had an obvious jump phenomenon during the start–stop phase. Meanwhile, thumbing changes occurred in the operation process. These problems would cause the motor to operate unstably in the end. Appl. Sci. 2019, 9, x FOR PEER REVIEW 31 of 36 closed-loop period higher than industrial demand in equipment commonly used in the industry. The fast closed-loop period means that the algorithm has higher computational efficiency. Since our algorithm could decompose complex inverse motion problems into several sub- problems and solve them one by one, all joint angles could be solved accurately and quickly. The final experimental results show that the system could complete the inverse solution of the kinematics within the specified period. This allows the algorithm to run stably in situations with real-time requirements. Figure 8 shows the end curve planned to use 15 S-curve segments. The apparent rise and fall process in the jerk curve could be seen. This allows the entire curve to have a smoother transition at start–stop. Based on Equation (46) and the plan in Figure 10, the angular curve of each joint could be obtained, as shown in Figures 11–16. It can be seen from Figures 11–16 that all joint motion curves, as well as the various derivative curves could be ensured to start from 0 during the start-up phase smoothly. The continuous and smooth was kept in the whole moving process. Moreover, the curve was smoothly reduced to zero during the stop phase. Especially the fourth derivative had kept the continuous and smooth fluctuations, which was the effect that could not be realized through traditional planning method. This paper introduced the joint curve obtained through using the traditional seven-segment planning there to facilitate the comparison, as shown in Figure 20–25. It can be clearly seen that the Jerk curve of each joint had an obvious jump phenomenon during the start–stop phase. Meanwhile, thumbing changes occurred in the operation process. These problems would cause the motor to operate Appl. Sci. 2019, 9, 4365 31 of 36 unstably in the end. (a) (b) (c) (d) Figure 20. (a) Displacement of axis I over time by using seven segments S-curve. (b) Velocity of axis I Figure 20. (a) Displacement of axis I over time by using seven segments S-curve. (b) Velocity of axis Appl. Sci. 2019, 9, x FOR PEER REVIEW 32 of 36 over time by using seven segments S-curve. (c) Acceleration of axis I over time by using seven segments I over time by using seven segments S-curve. (c) Acceleration of axis I over time by using seven S-curve. (d) Jerk of axis I over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis I over time by using seven segments S-curve. (a) (b) (c) (d) Figure 21. (a) Displacement of axis II over time by using seven segments S-curve. (b) Velocity of axis Figure 21. (a) Displacement of axis II over time by using seven segments S-curve. (b) Velocity of axis II over time by using seven segments S-curve. (c) Acceleration of axis II over time by using seven II over time by using seven segments S-curve. (c) Acceleration of axis II over time by using seven segments S-curve. (d) Jerk of axis II over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis II over time by using seven segments S-curve. (a) (b) (c) (d) Figure 22. (a) Displacement of axis III over time by using seven segments S-curve. (b) Velocity of axis III over time by using seven segments S-curve. (c) Acceleration of axis III over time by using seven segments S-curve. (d) Jerk of axis III over time by using seven segments S-curve. Appl. Sci. 2019, 9, x FOR PEER REVIEW 32 of 36 (a) (b) (c) (d) Figure 21. (a) Displacement of axis II over time by using seven segments S-curve. (b) Velocity of axis II over time by using seven segments S-curve. (c) Acceleration of axis II over time by using seven segments S-curve. (d) Jerk of axis II over time by using seven segments S-curve. Appl. Sci. 2019, 9, 4365 32 of 36 (a) (b) (c) (d) Figure 22. (a) Displacement of axis III over time by using seven segments S-curve. (b) Velocity of axis Figure 22. (a) Displacement of axis III over time by using seven segments S-curve. (b) Velocity of axis Appl. Sci. 2019, 9, x FOR PEER REVIEW 33 of 36 III over time by using seven segments S-curve. (c) Acceleration of axis III over time by using seven III over time by using seven segments S-curve. (c) Acceleration of axis III over time by using seven segments S-curve. (d) Jerk of axis III over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis III over time by using seven segments S-curve. (a) (b) (c) (d) Figure 23. (a) Displacement of axis IV over time by using seven segments S-curve. (b) Velocity of axis Figure 23. (a) Displacement of axis IV over time by using seven segments S-curve. (b) Velocity of axis IV over time by using seven segments S-curve. (c) Acceleration of axis IV over time by using seven IV over time by using seven segments S-curve. (c) Acceleration of axis IV over time by using seven segments S-curve. (d) Jerk of axis IV over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis IV over time by using seven segments S-curve. (a) (b) (c) (d) Figure 24. (a) Displacement of axis V over time by using seven segments S-curve. (b) Velocity of axis V over time by using seven segments S-curve. (c) Acceleration of axis V over time by using seven segments S-curve. (d) Jerk of axis V over time by using seven segments S-curve. Appl. Sci. 2019, 9, x FOR PEER REVIEW 33 of 36 (a) (b) (c) (d) Figure 23. (a) Displacement of axis IV over time by using seven segments S-curve. (b) Velocity of axis IV over time by using seven segments S-curve. (c) Acceleration of axis IV over time by using seven segments S-curve. (d) Jerk of axis IV over time by using seven segments S-curve. Appl. Sci. 2019, 9, 4365 33 of 36 (a) (b) (c) (d) Figure 24. Figure 24. (a) Displacem (a) Displacement ent of of ax axis is V over ti V over time me by us by using ing seven s seven segments egments S-curve. S-curve. (b) ( V b elocity ) Velocity of axis of axis Appl. Sci. 2019, 9, x FOR PEER REVIEW 34 of 36 V over time by using seven segments S-curve. (c) Acceleration of axis V over time by using seven V over time by using seven segments S-curve. (c) Acceleration of axis V over time by using seven segments S-curve. (d) Jerk of axis V over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis V over time by using seven segments S-curve. (a) (b) (c) (d) Figure 25. (a) Displacement of axis VI over time by using seven segments S-curve. (b) Velocity of axis Figure 25. (a) Displacement of axis VI over time by using seven segments S-curve. (b) Velocity of axis VI over time by using seven segments S-curve. (c) Acceleration of axis VI over time by using seven VI over time by using seven segments S-curve. (c) Acceleration of axis VI over time by using seven segments S-curve. (d) Jerk of axis VI over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis VI over time by using seven segments S-curve. Figure 18 shows the actual run in Cartesian space. It could be seen that the actual motion trajectory Figure 18 shows the actual run in Cartesian space. It could be seen that the actual motion coincided with the planned motion trajectory. The mean square error of the two tracks was calculated trajectory coincided wi 10 th the planned motion trajectory. The mean square error of the two tracks was to be 7.8265 10 m. This data shows that the error between the actual trajectory and the target −10 calculated to be 7.8265 × 10 m. This data shows that the error between the actual trajectory and the trajectory was extremely small. This shows that the inverse solution of the robot had higher accuracy. target trajectory was extremely small. This shows that the inverse solution of the robot had higher accuracy. 6. Conclusions Based on the DH model, this study proposed a universal algorithm for finding an inverse kinematics closed-form solution. This algorithm divided the inverse kinematics problem related to robots with a closed-form solution into three sub-problems assuming that the algebraic equation had a formula solution. The solvability conditions for the three types of sub-problems were analyzed to derive a formula solution. If a serial robot can be described with these three types of sub-problems, the robot will definitely have a closed-form solution. Meanwhile, the formula solution based on such a problem could quickly solve for all the joint angles. This method was not only applicable to six- DOF robots with a closed-form solution but also to low-DOF robots with the same solution form. In addition, establishing an algorithm based on the DH model provides a concise and efficient tool for selecting sub-problems and a real-time inverse kinematics solution for the motion controller of a serial robot. To verify the correctness and real-time performance of the algorithm, we presented four experimental designs. The first three experiments were conducted on MATLAB with a series of robot to verify the completeness, universality, and continuity of this algorithm. In the first experiment, the completeness of the algorithm was verified. The experiment used a low-degree-of-freedom robot with closed inverse solutions to prove that the algorithm could solve its closed inverse solution, and the algorithm would be terminated for the robot without closed inverse solutions to avoid infinite loop. This was used to prove the completeness of the algorithm. In the second experiment, two uncommon robots were constructed through using some basic problems, but the closed inverse solution could still be solved. Therefore, it could be known that the closed inverse solution of the series robots constructed by basic problems could be solved. Meanwhile, for these robots whose structure was not changed but the link parameter was changed, the closed inverse solution of them could also be solved correctly, which indicates that the algorithm had certain versatility in solving the closed inverse solution problem. In the third experiment, the common Puma560 robot was used as the experimental object. In the experiment, a spatial curve was planned. Appl. Sci. 2019, 9, 4365 34 of 36 6. Conclusions Based on the DH model, this study proposed a universal algorithm for ﬁnding an inverse kinematics closed-form solution. This algorithm divided the inverse kinematics problem related to robots with a closed-form solution into three sub-problems assuming that the algebraic equation had a formula solution. The solvability conditions for the three types of sub-problems were analyzed to derive a formula solution. If a serial robot can be described with these three types of sub-problems, the robot will deﬁnitely have a closed-form solution. Meanwhile, the formula solution based on such a problem could quickly solve for all the joint angles. This method was not only applicable to six-DOF robots with a closed-form solution but also to low-DOF robots with the same solution form. In addition, establishing an algorithm based on the DH model provides a concise and ecient tool for selecting sub-problems and a real-time inverse kinematics solution for the motion controller of a serial robot. To verify the correctness and real-time performance of the algorithm, we presented four experimental designs. The ﬁrst three experiments were conducted on MATLAB with a series of robot to verify the completeness, universality, and continuity of this algorithm. In the ﬁrst experiment, the completeness of the algorithm was veriﬁed. The experiment used a low-degree-of-freedom robot with closed inverse solutions to prove that the algorithm could solve its closed inverse solution, and the algorithm would be terminated for the robot without closed inverse solutions to avoid inﬁnite loop. This was used to prove the completeness of the algorithm. In the second experiment, two uncommon robots were constructed through using some basic problems, but the closed inverse solution could still be solved. Therefore, it could be known that the closed inverse solution of the series robots constructed by basic problems could be solved. Meanwhile, for these robots whose structure was not changed but the link parameter was changed, the closed inverse solution of them could also be solved correctly, which indicates that the algorithm had certain versatility in solving the closed inverse solution problem. In the third experiment, the common Puma560 robot was used as the experimental object. In the experiment, a spatial curve was planned. The correct joint angle sequence was obtained and the changes of the joint angle curve were continuously without jumping after been inversely solved by the algorithm. Which indicates that the algorithm could guarantee the continuity of its mapping on the inverse solution problem. The fourth experiment was conducted on a six-DOF robotic platform built using a Beckhoff-C6920 controller and an RGM motor to verify the real-time performance of the algorithm using the above hardware platform. The spatial and joint positions were computed with a close-loop cycle of 2 ms and were transmitted to the motor via CanOpen. The results demonstrated the ability of the algorithm to solve the three sub-problems within a specified cycle and to compute the appropriate joint angles. Moreover, the curve was continuous. This paper used a completely new approach to study the inverse kinematics of serial robots. In this study, the conditions for the existence of the closed-form inverse kinematic solution of a serial robot were completed and the basic theory of robotics was perfected. Based on this method, a general closed-form inverse kinematics algorithm based on the DH model was implemented, which had not been done in previous studies. Importantly, this method is a general-purpose algorithm that can be used in industrial fields in real time. This increases the versatility of the universal serial robot motion controller. However, the algorithm still has shortcomings. First, the algorithm only targets serial robots and the moving joints are not included in the solution model. This limits the scope of application of the algorithm, due to its inability to solve the cases of SCARA robots or other parallel robots. In addition, the algorithm cannot solve the problem when the robot passes through a singular point; singular points are avoided via restrictions implemented in the software. Moreover, because the algorithm is a purely analytical method, it is dicult to combine with other numerical methods to solve the problem of singular points. Author Contributions: Conceptualization, W.S.; methodology, W.S. and L.X.; software, W.S.; writing—original draft preparation, W.S.; writing—review and editing, W.S., L.X., H.B., L.Q.; supervision, L.Q., funding acquisition, L.X. Appl. Sci. 2019, 9, 4365 35 of 36 Funding: Supported by: National Key R&D Program of China (2016YFC0803000, 2016YFC0803005). Conﬂicts of Interest: The authors declare there is no conﬂict of interest regarding the publication of this paper. References 1. Xiao, W.; Strauß, H.; Loohß, T.; Homeister, H.W.; Hesselbach, J. Closed-form inverse kinematics of 6R milling robot with singularity avoidance. Prod. Eng. 2011, 5, 103–110. [CrossRef] 2. Wang, L.; Fallavollita, P.; Zou, R.; Chen, X.; Weidert, S.; Navab, N. Closed-form inverse kinematics for interventional C-arm X-ray imaging with six degrees of freedom: Modeling and application. IEEE Trans. Med. Imaging 2012, 31, 1086–1099. [CrossRef] [PubMed] 3. Khan, A.; Cheng, X.; Zhang, X.; Quan, W.L. Closed form inverse kinematics solution for 6-DOF underwater manipulator. In Proceedings of the 2015 International Conference on Fluid Power and Mechatronics (FPM), Harbin, China, 5–7 August 2015; pp. 1171–1176. 4. Bunathuek, N.; Laksanacharoen, P. Inverse kinematics analysis of the three-legged reconﬁgurable spherical robot II. In Proceedings of the 2017 3rd International Conference on Control, Automation and Robotics (ICCAR), Nagoya, Japan, 22–24 April 2017; pp. 31–35. 5. Bai, L; Yang, J.; Chen, X.; Jiang, P.; Liu, F.; Zheng, F.; Sun, Y. Solving the Time-Varying Inverse Kinematics Problem for the Da Vinci Surgical Robot. Appl. Sci. 2019, 9, 546. [CrossRef] 6. Hartenberg, R.S. A Kinematic Notation for Lower-Pair Mechanism Based on Matrices. Trans. ASME J. Appl. Mech. 1955, 22, 215–221. 7. Raghaven, M.; Roth, B. Kinematic analysis of the 6R manipulator of general geometry. In Proceedings of the International Symposium on Robotics Research, Hanoi, Vietnam, 6–10 October 2019; pp. 263–269. 8. Penrose, R. On Best Approximate Solutions of Linear Matrix Equations. Proc. Camb. Philos. Soc. 1956, 52, 17. [CrossRef] 9. Siciliano, B. A Closed-loop Inverse Kinematic Scheme for On-line Joint-based Robot Control. Robotica 1990, 8, 231–243. [CrossRef] 10. Wampler, C.W.I. Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods. IEEE Trans.Syst. Man Cybern. 1986, 16, 93–101. [CrossRef] 11. Kelemen, M.; Virgala, I.; Lipták, T.; Miková, L’.; Filakovský, F.; Bulej, V. A Novel Approach for a Inverse Kinematics Solution of a Redundant Manipulator. Appl. Sci. 2018, 8, 2229. [CrossRef] 12. Reiter, A.; Muller, A.; Gattringer, H. On Higher Order Inverse Kinematics Methods in Time-Optimal Trajectory Planning for Kinematically Redundant Manipulators. IEEE Trans. Ind. Inform. 2018, 14, 1681–1690. [CrossRef] 13. Feng, Y.; Wang, Y.; Wei, S. A novel hybrid electromagnetism-like algorithm for solving the inverse kinematics of robot. Ind. Robot 2011, 38, 429–440. [CrossRef] 14. Yin, F.; Wang, Y.N.; Wei, S.N. Inverse Kinematic Solution for Robot Manipulator Based on Electromagnetism-like and Modified DFP Algorithms. Acta Autom. Sin. 2011, 37, 74–82. [CrossRef] 15. Paul, R.P.; Shimano, B. Kinematic Control Equations for Simple Manipulators. In Proceedings of the IEEE Conference on Decision and Control Including the 17th Symposium on Adaptive Processes, San Diego, CA, USA, 10–12 January 1979. 16. Pieper, D.L. The Kinematics of Manipulators under Computer Control. Ph.D. Thesis, Stanford University, Stanford, CA, USA, 1968. 17. John, J.C. Inverse kinematics of the manipulator. In Introduction to Robotics: Mechanics and Control; Pearson: London, UK, 2005; pp. 87–88. 18. Siciliano, B.; Sciavicco, L.; Villani, L.; Oriolo, G. Robotics: Modelling, Planning and Control, 2nd ed.; Springer Publishing Company, Incorporated: London, UK, 2010; pp. 76–82. 19. Cui, H.-X.; Feng, K.; Li, H.-L.; Han, J.-H. Singularity avoidance of 6R decoupled manipulator using improved Gaussian distribution damped reciprocal algorithm. Ind. Robot 2017, 44, 324–332. [CrossRef] 20. Murray, R.M.; Sastry, S.S.; Li, Z. A Mathematical Introduction to Robotic Manipulation; CRC Press, Inc.: Florida, FL, USA, 1994; p. 292. 21. Kahan, W. Lectures on Computational Aspects of Geometry; University of California: Berkeley, CA, USA, 1983. 22. Paden, B. Kinematics and Control Robot Manipulators. Ph.D. Thesis, Department of Electrical Engineering and Computer Sciences, University of California, Berkeley, CA, USA, 1986. Available online: https://10.1109/ACSSC. 1985.671441 (accessed on 8 October 2018). [CrossRef] Appl. Sci. 2019, 9, 4365 36 of 36 23. Wang, H.; Lu, X.; Cui, W.; Zhang, Z.; Li, Y.; Sheng, C. General inverse solution of six-degrees-of freedom serial robots based on the product of exponentials model. Assem. Autom. 2018, 38, 361–367. [CrossRef] 24. An, H.S.; Seo, T.W.; Lee, J.W. Generalized solution for a sub-problem of inverse kinematics based on product of exponential formula. J. Mech. Sci. Technol. 2018, 32, 2299–2307. [CrossRef] 25. Corke, P.I.J.I.R.; Magazine, A. A robotics toolbox for MATLAB. IEEE Robot. Autom. Mag. 1996, 3, 24–32. [CrossRef] 26. Jazar, R.N. Inverse kinematics. In Theory of Applied Robotics; Springer: Berkeley, CA, USA, 2010; pp. 222–234. © 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/). http://www.deepdyve.com/assets/images/DeepDyve-Logo-lg.png Applied Sciences Multidisciplinary Digital Publishing Institute http://www.deepdyve.com/lp/multidisciplinary-digital-publishing-institute/existence-conditions-and-general-solutions-of-closed-form-inverse-fpN9NUDwiP

Loading next page...

References (31)

B. Siciliano (1990)
A closed-loop inverse kinematic scheme for on-line joint-based robot control
Robotica, 8
(2005)
Inverse kinematics of the manipulator
Alexander Reiter, A. Müller, H. Gattringer (2018)
On Higher Order Inverse Kinematics Methods in Time-Optimal Trajectory Planning for Kinematically Redundant Manipulators
IEEE Transactions on Industrial Informatics, 14
Feng Yin, Yaonan Wang, Shuning Wei (2011)
Inverse Kinematic Solution for Robot Manipulator Based on Electromagnetism-like and Modified DFP Algorithms: Inverse Kinematic Solution for Robot Manipulator Based on Electromagnetism-like and Modified DFP Algorithms
Acta Automatica Sinica, 37
Min Wu, N. Arbouz (1985)
Control Of Robot Manipulators
Nineteeth Asilomar Conference on Circuits, Systems and Computers, 1985.
R. Murray, S. Sastry, Zexiang Li (1994)
A Mathematical Introduction to Robotic Manipulation
Hong-Xin Cui, K. Feng, Huanliang Li, Jinhua Han (2017)
Singularity avoidance of 6R decoupled manipulator using improved Gaussian distribution damped reciprocal algorithm
Ind. Robot, 44
B. Paden (1985)
Kinematics and Control of Robot Manipulators
(1983)
Lectures on Computational Aspects of Geometry
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo (2008)
Robotics: Modelling, Planning and Control
W. Xiao, H. Strauss, Torsten Loohß, H. Hoffmeister, J. Hesselbach (2011)
Closed-form inverse kinematics of 6R milling robot with singularity avoidance
Production Engineering, 5
Lejing Wang, P. Fallavollita, Rui Zou, Xin Chen, S. Weidert, Nassir Navab (2012)
Closed-Form Inverse Kinematics for Interventional C-Arm X-Ray Imaging With Six Degrees of Freedom: Modeling and Application
IEEE Transactions on Medical Imaging, 31
F. Yin, Yaonan Wang, Shuning Wei (2011)
A novel hybrid electromagnetism-like algorithm for solving the inverse kinematics of robot
Ind. Robot, 38
Morten Engell-Nørregård, Kenny Erleben, Michael Satan, Raffaello Sanzio (2018)
Inverse Kinematics
H. An, T. Seo, Jehwon Lee (2018)
Generalized solution for a sub-problem of inverse kinematics based on product of exponential formula
Journal of Mechanical Science and Technology, 32
L. Bai, Jianxing Yang, Xiaohong Chen, Pei Jiang, Fuqiang Liu, Fan Zheng, Yuanxi Sun (2019)
Solving the Time-Varying Inverse Kinematics Problem for the Da Vinci Surgical Robot
Applied Sciences
M. Kelemen, Ivan Virgala, T. Lipták, Ľ. Miková, Filip Filakovský, V. Bulej (2018)
A Novel Approach for a Inverse Kinematics Solution of a Redundant Manipulator
Applied Sciences
Natthaphon Bunathuek, Pudit Laksanacharoen (2017)
Inverse kinematics analysis of the three-legged reconfigurable spherical robot II
2017 3rd International Conference on Control, Automation and Robotics (ICCAR)
C. Wampler (1986)
Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods
IEEE Transactions on Systems, Man, and Cybernetics, 16
Feng Yin, Yaonan Wang, Shuning Wei (2011)
Inverse Kinematic Solution for Robot Manipulator Based on Electromagnetism-like and Modified DFP Algorithms
Acta Automatica Sinica, 37
(1983)
Lectures on Computational Aspects of Geometry; University of California: Berkeley
Haixia Wang, Xiao Lu, Wei Cui, Zhiguo Zhang, Yuxia Li, Chunyang Sheng (2018)
General inverse solution of six-degrees-of-freedom serial robots based on the product of exponentials model
Assembly Automation
J. Denavit (1955)
A kinematic notation for lower pair mechanisms based on matrices
, 22
M. Raghaven, B. Roth (1991)
Kinematic analysis of the 6R manipulator of general geometry
Donald Peiper (1968)
THE KINEMATICS OF MANIPULATORS UNDER COMPUTER CONTROL
(2005)
Inverse kinematics of the manipulator. In Introduction to Robotics: Mechanics and Control; Pearson: London, UK
Peter Corke (1996)
A robotics toolbox for MATLAB
IEEE Robotics Autom. Mag., 3
(2010)
Robotics: Modelling, Planning and Control, 2nd ed.
R. Paul, B. Shimano (1978)
Kinematic control equations for simple manipulators
1978 IEEE Conference on Decision and Control including the 17th Symposium on Adaptive Processes
R. Penrose (1956)
On best approximate solutions of linear matrix equations
Mathematical Proceedings of the Cambridge Philosophical Society, 52
Asghar Khan, C. Xiangming, Zhang Xingxing, W. Quan (2015)
Closed form inverse kinematics solution for 6-DOF underwater manipulator
2015 International Conference on Fluid Power and Mechatronics (FPM)

Publisher: Multidisciplinary Digital Publishing Institute
Copyright: © 1996-2019 MDPI (Basel, Switzerland) unless otherwise stated Terms and Conditions Privacy Policy
ISSN: 2076-3417
DOI: 10.3390/app9204365
Publisher site: See Article on Publisher Site

Abstract

applied sciences Article Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots 1 2 , 1 3 Wang Shanda , Luo Xiao *, Luo Qingsheng and Han Baoling School of Mechatronic Engineering, Beijing Institute of Technology, Beijing 100081, China; 3120160137@bit.edu.cn (S.W.); luoqsh@bit.edu.cn (Q.L.) School of Computer Science & Technology, Beijing Institute of Technology, Beijing 100081, China School of Mechanical Engineering, Beijing Institute of Technology, Beijing 100081, China; hanbl@bit.edu.cn * Correspondence: luox@bit.edu.cn; Tel.: +86-010-6891-8856 Received: 14 August 2019; Accepted: 10 October 2019; Published: 16 October 2019 Featured Application: This paper primarily studies the existence conditions for the closed-form inverse kinematic solution for revolute serial robots. Based on the results, a general closed-form inverse solution algorithm is designed. Abstract: This study proposes a method for judging the existence of closed-form inverse kinematics solutions based on the Denavit–Hartenberg (DH) model. In this method, serial robots with closed-form solutions are described using three types of sub-problems from the viewpoint of solving algebraic equations. If a serial robot can be described using these three types of sub-problems, i.e., if the inverse kinematics problems can be solved by several basic problems, then there is a closed-form solution. Based on the above method, we design a set of universal closed-form inverse kinematics solving algorithms. Since there is a deﬁnite formula solution for the three types of sub-problems, the joint angles can be rapidly determined. In addition, because the DH parameters can directly reﬂect the linkage of the robot, the judgment of the sub-problems is also quick and accurate. More importantly, the algorithm can be applied to serial robots with low degrees of freedom. This enables the algorithm to not only quickly and accurately solve inverse kinematics problems but also to exhibit high universality. This proposed theory improves the existence conditions for closed-form reverse solutions and further promotes the development of motion control techniques for serial robots. Keywords: inverse kinematics; Denavit–Hartenberg method; closed-form solution; existence condition; universal inverse kinematics algorithm 1. Introduction Recently, the use of serial robots has increased owing to their extensive application in the field of bionics and in various industries [1–5]. Overcoming the usual problems in inverse kinematics is a key to controlling the motion of serial robots. However, inverse kinematics is a non-linear problem with multiple solutions. Among these solutions, the general numerical solution method is both time-consuming and unstable. Alternatively, the closed-form solution for inverse kinematics is commonly sought for practical applications but has two major limitations that remain unsolved. First, there is no general method for finding the closed-form solution. Second, the Pieper criterion is not complete. To resolve these problems, a model for the robotic kinematics needs to be established. There are two major methods for robotic modeling using inverse kinematics, namely the Denavit–Hartenberg (DH) parametric method and the product of exponentials (POE) formula method. Appl. Sci. 2019, 9, 4365; doi:10.3390/app9204365 www.mdpi.com/journal/applsci Appl. Sci. 2019, 9, 4365 2 of 36 The DH parametric modeling method is based on a transformation of coordinates [6]. Due to its easily visualized parameters, this convenient method has attracted the attention of many researchers and engineers. Under this model, the inverse kinematics problem is solved via two methods, i.e., numerical and closed-form solutions. Moreover, because the forward kinematics formula of the DH model provides a maximum of six independent equations, simplifying the algebraic equation using the numerical method can solve the inverse kinematics problem for most non-redundant robots [7]. In addition, the Jacobian matrix derived in the DH model can be used to solve the inverse kinematics problem for redundant robots [6,8–11]. Based on the numerical method, a series of objective functions can be added [12] that not only enable the robot to reach a designated position but also allow the completion of some extra tasks, such as keeping away from a singular point or maintaining its maximum motion ability. Studies on numerical solutions have primarily focused on eciently and stably solving the equations or obtaining an approximate solution using the functional approximation method in combination with neural networks and intelligent algorithms [11,13,14]. However, owing to the existence of the robotic singular posture, the stability of the numerical solution and its convergence rate cannot be guaranteed. Therefore, this method is not suitable for cases where instantaneity is required. Another method based on the DH model is the analytical method, which uses a closed formula to determine the relations between the terminal posture and various joint angles. Nonetheless, not all robots have a closed-form solution. In 1978, a general method for solving for the closed-form solution was proposed by Paul [15]. In 1968, Pieper studied six-degree-of-freedom (DOF) robots with three axes intersecting at one point [16] and concluded that a serial robot with the three terminal axes intersecting at one point has a closed-form solution. This criterion, i.e., that a six-DOF serial robot with three terminal axes intersecting at one point (deﬁned as Pieper Criterion 1, let us say PC-1) or three adjacent parallel axes (deﬁned as Pieper Criterion 2, let us say PC-2) has a closed-form solution, has been obtained in subsequent studies [17,18]. However, the method proposed by Paul needs to be manually derived and the formula needs to be rederived every time the robot structure changes, which limits the universality of the closed-form solution. Accordingly, studies on the closed-form solution based on the DH model have primarily focused on situations with already known structures, such as keeping the robot away from a singular value [19]. More importantly, the necessary conditions for the existence of the closed-form inverse solution given by Pieper are inadequate. For example, a robot with the DH parameters listed in Table 1 has three terminal axes intersecting at one point, as well as three parallel joints; however, its inverse kinematics problem cannot be solved. Table 1. Counter-example of Pieper Criterion 1 (PC-1). n / d/mm a/mm / 1 d a 0 1 1 1 2 d a 0 2 2 2 3 d a /2 3 3 3 4 d 0 /2 4 4 5 0 0 /2 6 0 0 0 The DH method is a parameter selection norm based on a transformation of coordinates [6]. This method regulates the criterion for selecting three parameters between two adjacent axes, where a represents the linkage oset, i.e., the normal oset between the two adjacent axes, d is the linkage length, representing the axial oset between the two adjacent axes, and is the linkage torsion, representing the included angle between the two adjacent axes. On a similar note, the robot with the DH parameters listed in Table 2 has three adjacent and parallel joints; however, we cannot solve the inverse kinematics problem. Appl. Sci. 2019, 9, 4365 3 of 36 Table 2. Counter-example of Pieper Criterion 2 (PC-2). n / d/mm a/mm / 1 d 0 0 1 1 2 0 a 0 2 2 3 0 a /2 3 3 4 d 0 0 4 4 5 d a 0 5 5 5 6 d 0 0 6 6 The inadequacy of the Pieper principle leads to defects in the inverse kinematics algorithm designed on the basis of this principle. However, no further studies on this problem have been conducted to supplement and improve this method. Conversely, the modeling method for the POE formula has been presented in some detail [20]. The logic for the inverse kinematics solution problem based on POE primarily consists of a reduction of the original problem into several sub-problems followed by solving all the joint angles step by step. This method uses an abstract geometric model to solve the inverse kinematics problem, therefore enhancing the generalization of the algorithm. In POE-based studies, several universal inverse kinematics algorithms have been designed, of which the most typical universal sub-problem was proposed in 1986 [21,22]. There is subproblem 1. Rotation about a single axis, subproblem 2. Rotation about two subsequent axes, and subproblem 3. Rotation to a given distance. A robotic universal solving method that meets the Pieper principle has been proposed [23], and a new sub-problem was proposed to solve the universal inverse kinematics algorithm [24]. The universal inverse kinematics algorithm based on the POE model has deﬁnite geometrical signiﬁcance and stable numerical values. However, in practice, it is impossible for some robots to utilize the known sub-problems to describe and solve the problem. In addition, the algorithm consumes enormous computational resources when selecting sub-problems. As a result, it is dicult for the universal inverse solution algorithm based on the POE model to be applied in cases with real-time requirements. It can be seen that both the DH model and POE model have various drawbacks in solving inverse kinematics problems. This study proposes a method for judging the existence of closed-form solutions based on the DH model to deal with the ﬁrst problem of the closed-form solution. In this method, revolute serial robots with closed-form solutions are described using three types of sub-problems from the viewpoint of solving the algebraic equations. They are sub-problem of translation components, sub-problem of rotational components, and sub-problem of sub-chains. In more detail, based on sub-problems, when the conﬁguration of the robot satisﬁes some characteristics, some more concise and ﬁxed formulas can be used to solve the joint angle. This article refers to this more speciﬁc situation as the basic problem of sub-problems, a total of ten. If a serial robot can be described using the three types of sub-problems, i.e., if the inverse motion problems can be solved by several basic problems, then there is a closed-form solution. Based on the above method, a set of universal closed-form inverse kinematics solving algorithms is designed to address the second problem mentioned above. Since there is a deﬁnite formula solution in the three types of sub-problems, the joint angles can be rapidly determined. In addition, because the DH parameters can directly reﬂect the linkage of the robot, the judgment of the sub-problems is also quick and accurate. In general, the main contributions of this article are: 1. Proposes a more complete judgment condition for the existence of the closed-form solution of series robot to solve the defect of Pieper principle. In addition, this judgment method is a method that is suitable for all robots whose degree of freedom is less than six. 2. A general inverse kinematics algorithm is designed for this judgment method. We can use this algorithm to ﬁnd the angles of all joints of the series robots meet the conditions quickly and accurately. Appl. Sci. 2019, 9, 4365 4 of 36 3. This universal inverse solution algorithm has a simple judgment method, the calculation speed of it is fast, and it is an algorithm that can be applied in occasions with real-time requirements. Usually, a special method is used for the motion control of series robot, only the common Appl. Sci. 2019, 9, x FOR PEER REVIEW 4 of 36 conﬁguration is supported, and some parameters cannot be adjusted. Therefore, the proposed method can greatly improve the application range of the motion controller of series robots. configuration is supported, and some parameters cannot be adjusted. Therefore, the proposed 4. Proposed method and theory has also enriched the foundation of robotics. method can greatly improve the application range of the motion controller of series robots. 4. Proposed method and theory has also enriched the foundation of robotics. The manuscript is organized as follows. Section 2 derives a kinematics equation for a robot based on the standard DH model. Relevant properties are obtained according to the forward kinematics The manuscript is organized as follows. Section 2 derives a kinematics equation for a robot based equation to be used in the inverse kinematics problems. Section 3 analyzes the robotic inverse kinematics on the standard DH model. Relevant properties are obtained according to the forward kinematics problems. According to the existence conditions, the inverse kinematics problems are divided into equation to be used in the inverse kinematics problems. Section 3 analyzes the robotic inverse three sub-problems and 10 types of basic problems. Section 4 designs a universal inverse kinematics kinematics problems. According to the existence conditions, the inverse kinematics problems are algoridivide thm. S d into ection th5 ree sub-pro designedbtlems and wo expe 10 rim types ents. oT f h be asf ii c problems. rst experim Se en ct tion us 4 esdM esA igns TL A a univer B andsa Rlo in bo verse tics Toolbo kinem x [25] atics to t algorithm. est the com Sec ple tt ion 5 eness de , v signe ersad ti two experiments. lity, and continui tThe y of first the a exper lgoriitmen hm.t use Subss e MATL quentAB and ly, the Robotics Toolbox [25] to test the completeness, versatility, and continuity of the algorithm. algorithm is implemented and verified in a real-time system to demonstrate its correctness and real-time Subsequently, the algorithm is implemented and verified in a real-time system to demonstrate its stability. Section 6 presents a summary of the findings of this paper. correctness and real-time stability. Section 6 presents a summary of the findings of this paper. 2. Kinetic Models 2. Kinetic Models 2.1. Standard DH Parameters 2.1. Standard DH Parameters The DH method is a parameter selection norm based on a transformation of coordinates [6]. The DH method is a parameter selection norm based on a transformation of coordinates [6]. Let axis k denote the axis of the axis connecting link k 1 to link k; DH method adopted to deﬁne Let axis 𝑘 denote the axis of the axis connecting link 𝑘− 1 to link 𝑘 ; DH method adopted to link Frame k: define link Frame 𝑘 : Choose axis z along the axis of joint k + 1. Locate the origin O at the intersection of axis z with k k k Choose axis 𝑧 along the axis of joint 𝑘+ 1 . Locate the origin 𝑂 at the intersection of axis the common normal to axes z and z . Locate also O at the intersection of the common normal with k1 k k0 𝑧 with the common normal to axes 𝑧 and 𝑧 . Locate also 𝑂 at the intersection of the common axis z . Choose axis x along the common normal to axes z and z with direction from joint k to k1 k k1 k normal with axis 𝑧 . Choose axis 𝑥 along the common normal to axes 𝑧 and 𝑧 with direction joint k + 1. Choose axis y so as to complete a right-handed frame. from joint 𝑘 to joint 𝑘+ 1 . Choose axis 𝑦 so as to complete a right-handed frame. Once the link frames have been established, the position and orientation of Frame k with respect Once the link frames have been established, the position and orientation of Frame 𝑘 with to Frame k 1 are completely speciﬁed by the following parameters: a distance between O and O k k k0 respect to Frame 𝑘− 1 are completely specified by the following parameters: 𝑎 distance between d coordinate of O along z , angle between axes z and z about axis x to be taken positive k k0 k1 k1 k k 𝑂 and 𝑂 𝑑 coordinate of 𝑂 along 𝑧 , 𝛼 angle between axes 𝑧 and 𝑧 about axis 𝑥 to when rotation is made counter-clockwise, and angle between axes x and x about axis z to be k k1 k k1 be taken positive when rotation is made counter-clockwise, and 𝜃 angle between axes 𝑥 and 𝑥 taken positive when rotation is made counter-clockwise. about axis 𝑧 to be taken positive when rotation is made counter-clockwise. Using the DH method, the relations for a robot can be directly established in a visualized manner Using the DH method, the relations for a robot can be directly established in a visualized manner and the existence conditions for closed-form solutions can be derived in a more direct and visualized and the existence conditions for closed-form solutions can be derived in a more direct and visualized manner. The standard DH parameters in the case of the KingKong collaborative robot are shown in manner. The standard DH parameters in the case of the KingKong collaborative robot are shown in Table 3. A schematic diagram of the robot is shown in Figure 1. Table 3. A schematic diagram of the robot is shown in Figure 1. 𝑗 𝑧 𝑦 𝑥 6 6 3 6 Joint 6 𝑥 4 𝑗 𝑧 𝑥 4 5 Joint 4 Joint 5 𝑥 𝑦 𝑗 5 5 Joint 3 𝑥 2 𝑧 Joint 2 Joint 1 𝑥 𝑦 𝑦 1 1 0 (a) (b) Figure 1. (a) Schematic for the KingKong parameters: 𝑑 is the linkage length of the 𝑘 th link and 𝑎 Figure 1. (a) Schematic for the KingKong parameters: d is the linkage length of the kth link and a is k k is the linkage torsion of the 𝑘 th link. 𝑗 is the 𝑘 th joint. (b) Visualization of the KingKong model. the linkage torsion of the kth link. j is the kth joint. (b) Visualization of the KingKong model. Appl. Sci. 2019, 9, 4365 5 of 36 Table 3. Denavit–Hartenberg (DH) parameters of the KingKong collaborative robot. n / d/mm a/mm / 1 89.459 0 /2 2 0 42.5 0 3 0 39.225 0 4 109.15 0 /2 5 94.65 0 /2 6 82.3 0 0 2.2. Forward Kinematics Formula Using the following formula, the DH parameters [6] can be transformed into elements of a transformation matrix from [6]: 2 3 cos sin cos sin sin a cos 6 k k k k k k k 7 6 7 " # 6 7 6 7 6 7 R P sin cos cos cos sin a sin 6 7 k1 k k k k k k k k k 6 7 ( ) T = = 6 7, (1) k 13 6 7 6 7 0 1 0 sin cos d 6 k k k 7 6 7 4 5 0 0 0 1 k1 k1 where k indicates the number of the current joint. R is the rotational component of T( ), a k k k1 k1 3 3 orthogonal matrix, and P is the translational component of T( ). The end transformation k k matrix of the end executor of the robot relative to the coordinate system can be obtained using the following formula: 0 0 1 2 i1 T = T T T T . (2) i 1 2 3 i i indicates the number of degrees of freedoms of the robot. According to Equation (2) from [18], we could obtain T as follows: 2 3 n=i n=i " # Q P 6 7 0 0 n1 0 i1 6 7 R P 6 R R ::: P 7 0 6 n 7 i i 1 i 6 7 T = = , (3) 6 n=1 n=1 7 13 i 6 7 0 1 4 5 13 0 1 0 0 i1 where R is the rotational component of T . Since the rotational component R is closed and i i i 0 0 0 orthogonal, R is an orthogonal matrix. Here, P is the translational component of T , which can be i i i represented as follows: n=i n=i X X 0 0 i1 P = R ::: P = P . (4) i 1 i n=1 n=1 According to Equation (4), the positional component of the back linkage is inﬂuenced by the front joint angle. In the deﬁnition of the DH parameter, the Z-direction of the joint coordinate indicates the orientation of the joint and can be used to characterize the relationship between the joints. Here, the joints of the serial robots are represented as z . To concisely indicate the linkage relations, the following a,d deﬁnitions are made: = 0, then z k z , whereas = /2, then z ?z , where z represents k k k+1 k k k+1 the linkage parameters a = d = 0. k k 2.3. Decoupling of the End Linkage " # 0 0 R P i1 i1 The coordinate transformation matrix is written as T = . Moreover, the 13 i1 0 1 " # i1 i1 R P i1 i i transformation matrix of the last axis is T = . The rotational component R is written 13 0 1 h i ! ! ! in the form of a row vector as R = x y z . Appl. Sci. 2019, 9, 4365 6 of 36 Then, P can be written as: 2 3 6 cos 7 6 7 6 7 6 7 0 0 0 0 6 7 P = R6 sin 7a + z d + P, (5) i i i 6 7 i i1 i1 i1 6 7 4 5 further: 2 3 cos 6 7 6 7 6 7 6 7 0 0 6 7 R6 sin 7 = x . (6) 6 7 i1 i 6 7 4 5 The following conclusions can be drawn: ! ! 0 0 when = /2 then y = z , i i1 ! ! 0 0 0 0 P = P x a y d ; (7) i i i1 i i i ! ! 0 0 when = /2 then y = z , i i1 ! ! 0 0 0 0 P = P x a + y d ; (8) i i i1 i i i ! ! 0 0 and when = 0 then, z = z , i i1 ! ! 0 0 0 0 P = P x a z d . (9) i i i1 i i i The above derivation demonstrates that when the current end transformation matrix T is known, 0 0 the translational component P in the previous transformational matrix T can be obtained based i1 i1 0 0 0 on the rotational component R of T and the translational component P. Given such a property, the i i i last axis may not be considered when the translational component of the robot is being analyzed. 2.4. Translational Relation of the Rotational Component k1 According to Equation (3) form [6], R = Rotz( )Rotx( ). The rotational component of the k k transformational matrix can be expressed as: n=k n=k Y Y 0 n1 R = R = Rotz( )Rotx( ). (10) n n n=1 n=1 If there exists several continuous parallel joints in the robot, then z k z ::: k z and = l l+1 l ::: = = 0. In addition, the rotational transformation in the same direction has additivity: l+1 m1 n=m R = Rotz( )Rotx( ) = Rotz(Q)Rotx( ) = R , (11) n n m n=l where Q = + ::: + . Here, we refer to z k z ::: k z as a sub-chain s. This concept will be m m l l+1 l l+1 used in the following paragraphs. According to Equation (11), the translational components of the sub-chain z k z ::: k z can be deﬁned and simpliﬁed as follows: 2 m 2 3 2 3 2 3 6 a 7 6 0 7 6 a cos Q 7 m m 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 m1 6 7 6 7 6 7 P = R P = Rotz(Q)6 0 7 + 6 0 7 = 6 a sin Q 7 = P , (12) m m m 1.m1 6 7 6 7 6 7 s 6 7 6 7 6 7 4 5 4 5 4 5 0 d d m m where Q = + ::: + . 2 m 1 Appl. Sci. 2019, 9, 4365 7 of 36 R deﬁned by the formula (11) and P deﬁned by the formula (12), will be used in the following s s paragraphs. 2.5. Solving the Trigonometric Equation This section summarizes two common formula solutions for trigonometric functions. The ﬁrst can be derived using Equation (4): 2 3 1 1 1 6 P sin n P sin + P a + a cos 7 y 1 1 x 1 z 1 1 1 n=i 6 7 X i i i 6 7 6 7 0 0 0 1 n1 6 1 1 1 7 6 7 P = P + R R P = P a + a sin n P n P cos , (13) 6 z y x 7 n 1 1 1 1 1 1 i 1 1 6 i i i 7 6 7 4 5 1 1 1 n=2 sin P + n P + 1 + P d 1 x 1 y z 1 i i i 1 1 1 0 1 0 0 ( ) where P , P , and P represent the three components in R P P and n = sign . According x y z i i i i i 1 i 1 0 0 to Equation (18), P , P , and satisfy a special trigonometric function relation, as shown in x y 1 i i Equation (14): " # " #" # P D L sin x 1 = , (D , 0 or L , 0). (14) P L D cos Therefore, 0 0 0 0 = atan2 D P + L P , L P D P . (15) 1 x y x y i i i i Another common trigonometric equation is shown in Equation (16) from [17]: Acos + Bsin = C, (A , 0 or B , 0), (16) whose solution is: 0 1 2 2 2 B C B B + A C B C B C = 2atanB C. (17) @ A A + C Equations (15) and (17) can map the joint angles within the interval of [, ]. Moreover, in the following paragraphs, the robotic inverse solution equation is transformed into Equations (14) and (16), resulting in a ﬁxed formula, which can compute the equation parameters and further derive the common closed-form formula solution for serial robots. When the parameters for Equations (14) and (16) are zero, the equation has no solution. This provides a suitable answer to the question of whether a robot has a solution when the linkage parameters are sparse. For convenience, Equation (14) is represented as = F (D, L, x, y) and Equation (15) is represented as , = F (A, B, C). 1 1 k1 k2 2 3. Analysis of The Inverse Kinematics Problem " # 0 0 R P i i Given a transformational matrix with a known end T = , the joint angle is obtained 13 0 1 0 0 inversely from the equation provided by R and P; this is known as inverse kinematics. i i 3.1. Three Types of Sub-Problems Related to Closed-Form Inverse Kinematics The French mathematician Évariste Galois proved that the quartic polynomial equation has a closed-form solution, whereas the sextic polynomial equation generally has none. The rotational and translational components of the robotic end transformational matrix provide three independent equations separately. The two equation sets are combined to solve the inverse kinematics problem, and then high-order polynomial equations may be required. Therefore, a robot has a closed-form inverse solution, partial joint angles will be preferably solved from a certain equation set followed by the other joint angles. To guarantee the existence of a closed-form solution, a feasible solution guarantees mutual decoupling between the translational and rotational components [17,26]. Such traditional decoupling Appl. Sci. 2019, 9, 4365 8 of 36 enables the translational component to be associated with the front three joints, which in turn, are 0 3 solved via P. The latter three joints are solved via the rotational component R. 3 6 " #" # " # 0 0 3 0 3 0 R P R 0 R R P 3 3 6 3 6 3 T = = . (18) 0 1 0 1 0 1 Based on the derivation in Section 2.4, a robot has several parallel joints, the number of unknown numbers in the rotational component will decrease. Therefore, another feasible decoupling idea is to preferably ﬁnd a solution using the rotational component R(',#, ) and solve for the remaining joint angles using the translational component P. This strategy has not been mentioned in any previous studies. " # " # 0 0 0 0 R P R(',#, ) P 6 6 6 6 T = = . (19) 0 1 0 1 Based on the above two decoupling methods, we could obtain three types of sub-problems. In the ﬁrst type, when the number of unknowns in the rotational component is larger than three and the robot meets some structural characteristics, some joint angles can be preferably and directly solved using the translational component. This article refers to the ﬁrst type sub-problem as the sub-problem of translation components. In the second type, the robot has several parallel joints and the number of unknowns in the rotational component is no greater than 3, these unknowns can be solved using the rotational component. This article refers to the second type sub-problem as sub-problem of rotational components. In the third and final type, when the second type is used to solve the unknowns for the robot and the joint angles are still not completely solved, the remaining joint angles are obtained using the translational component. This article refers to the third type sub-problem as a sub-problem of sub-chains. Via an analysis of these sub-problems, it is possible to determine the solving conditions and methods in detail. In Sections 3.2–3.4, a more detailed analysis and derivation will be made for each sub-problem, and the formulas and conditions for solving the basic problems will be obtained. 3.2. The First Type of Sub-problem When there exist more than three unknowns in the rotational component, the partial joint angles should preferably be solved using the translational component. With reference to the DH parameter table, the DOF of the robot is i and the number of the DOF with = 0 in the ﬁrst i 1 DOFs is p. In the case of i p > 3, there are more than three unknowns in the rotational component. According to Equation (4), the back joint angles are inﬂuenced by the front joint angles in the translational component. Therefore, the association with a maximum of the front three joint angles in the translational equation must be ﬁrst guaranteed to solve the ﬁrst type of sub-problem. When the ﬁrst joint angle of the robot model Robot with i DOFs is solved, T can be calculated using the forward kinematics formula. Meanwhile, the ﬁrst row of the DH parameters is deleted to form a new robot model Robot and a new transformational matrix T : i1 i1 " # 0 1 0 1 0 R R P 1 0 1 0 0 1 1 1 T = T T = T . (20) i 1 i 13 i 0 1 There may only be a parallel or vertical relationship between two adjacent joints. Since only the linkage relation of the front three axes is considered, the possible conﬁguration between the ﬁrst three 1 1 axes is only the case where C C = 4. Moreover, z ?z ?z , z k z ?z , z ?z k z and z k z k z . 1 2 3 1 2 3 1 2 3 1 2 3 2 2 However, during the process of derivation, it was found that z k z k z only provides two valid 1 2 3 equations in X-direction and Y-direction, making it impossible to solve for three unknowns: Appl. Sci. 2019, 9, 4365 9 of 36 2 3 6 a cos( + + ) + a cos( + ) + a cos 7 3 1 2 3 2 1 2 1 1 6 7 6 7 6 7 6 7 P = a sin( + + ) + a sin( + ) + a sin . (21) 6 7 3 1 2 3 2 1 2 1 1 6 7 6 7 4 5 d + d + d 1 2 3 Such a case is the counter example mentioned in the preceding paragraphs. The Piper principle does not speciﬁcally describe such a case. Therefore, the linkage parametric limitations and the formula solution for the three basic problems are derived in detail in the following paragraphs. 3.2.1. First Three Joint Conﬁgured as z k z ?z 1 2 Here, the expression for the translational component is P = P + R P + R R P . However, 1 1 2 1 2 3 R R P contains the cubic term of the sine and cosine functions, which would directly increase the 1 2 3 order of the equation to sextic. Only when a = 0 will the requirements for a closed solution be met. According to Equation (18), The formula solution of can be obtained as follows: 0 0 > D = n d = F D, L, P , P 1 2 11 1 x y < 3 3 , where . (22) 0 0 0 2 0 2 2 L = P + P D = F D,L, P , P 12 1 x y x y 3 3 3 3 Once is obtained, the forward kinematics formula can be used to forward simplify the robot to obtain the expression of T and the DOFs of i 1. The basic problem can still be employed to seek a solution to . To guarantee the existence of only the front two joint angles in the translational equation, the linkage of z needs to satisfy d = a = 0. However, according to the end decoupling formula proposed k>3 in Section 2.3, the linkage parameters of the last joint are not restricted. 3.2.2. First Three Joint Conﬁgured as z k z ?z 1 2 3 The formula solution for z k z ?z is derived in the following paragraphs. Its translational 1 2 3 component can be written as P = P + P + R P , and its translational equation can be arranged as 1 12 12 3 follows: 2 3 2 3 6 a cos + D sin( + ) + L cos( + ) 7 6 P 7 1 1 1 2 1 2 x 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 a sin + L sin( + ) D cos( + ) 7 = 6 P 7. (23) 2 2 y 1 1 1 1 6 7 6 7 6 7 6 7 4 5 4 5 n a sin P d d 2 3 3 z 1 2 Arranging Equation (23) gives the formula solution Equation (24): A = 0 > 3 , = F (A , B , C ), where B = n a ; 31 32 2 3 3 3 3 2 3 C = P d d 3 1 2 A = P 1 x > 3 > B = P > 1 y D = n d 2 3 , = F (A , B , C ), where 11 12 2 1 1 1 (24) L = a + a cos 2 3 3 0 2 0 2 2 2 2 P + P +a L D > x y 3 3 C = 2a > g 0 0 P = P a cos x x 1 1 0g 0g 3 3 + = F D, L, P , P , where ; 1 2 1 x y 3 3 > : 0 0 P = P a sin y y 1 1 3 3 = f[( + ) ] ; 2 1 2 1 where f[] in the last equation indicates that is mapped onto the interval of [,]. Under such a structure, is preferably solved using the equation in the Z-direction. Therefore, the existence of z after z does not inﬂuence the solution of , resulting in the derivation of two 3 3 a a dierent structures, z k z ?z ?z and z k z ?z k z . 2 3 2 3 1 1 4 4 Appl. Sci. 2019, 9, 4365 10 of 36 According to the translational equation of z k z ?z ?z and Equation (23), the critical coefficient is: 1 2 3 D = n d + n d 2 3 2 4 . (25) L = a + a cos 2 3 3 Moreover, according to Equation (23), the critical coecient under z k z ?z k z is: 1 2 3 D = n d 2 3 A = n n d , where (26) 3 2 3 4 L = a + a cos + n d sin . 2 3 3 3 3 To guarantee that only the front three joint angles exist in the translational equation, the linkage of z needs to satisfy d = a = 0. There is not restricted to the linkage parameters of the last joint. k>4 In addition, in the basic problems mentioned above, when d = a = 0 is true in z , it can also 4 4 4 be used to solve z k z ?z . Similarly, it can be used to solve z k z . Therefore, such a type of basic 1 2 3 1 2 problem contains two sets of formula solutions that can be used to solve four cases. 3.2.3. First Three Joint Conﬁgured as z ?z k z 1 2 3 a a The solution method for z ?z k z in Section 3.4.1 also applies here. In addition, if z ?z k z ?z 1 2 1 2 3 3 4 and D = n (d + d ), can still be directly solved. This results in a special situation. Further, 1 2 3 1 suppose there is a robot with i degrees of freedom, in which the conﬁguration of the ﬁrst g joints is z ?z k ::: k z ?z , and the linkage of z needs to satisfy d = a = 0. These are not restricted to the 1 2 g1 k>g linkage parameters of the last joint. The formula solution of can be obtained as follows: g1 > P 0 0 > > D = n d = F D, L, P , P m 11 1 x y < g g , where m=2 . (27) 0 0 = F D,L, P , P 12 1 x y > g g 0 0 2 2 2 L = P + P D x y g g According to the forward kinematics formula, after is solved, T can be cancelled to allow the robot model to reduce a joint angle and become a new model. At that time, selective solving can be performed in all of the sub-problems. Basic problems 3.2.1 and 3.2.3 can be solved using the same formula solution. In the following paragraphs, basic problem 3.4.1 will be used for the description. Therefore, there exist two types of basic problems in the ﬁrst type of sub-problem. This section gives the corresponding relevant existence conditions and formula solutions. 3.3. Solving the Second Type of Sub-Problem Since the rotational component only provides three independent equations, a maximum of three unknowns can be solved. First, the rotational matrix Rotx() is introduced. Rotx() represents a radian rotation around the X-coordinate axis. Similarly, Rotz() and Roty() represent rotations around the Y- and Z-axes. The Euler angle theorem indicates that a generic rotation matrix can be obtained by composing a suitable sequence of three rotations while guaranteeing that two successive rotations are not made around parallel axes [18]. Therefore, any rotational matrix R can be expressed as Rotz('), Roty(#), and Rotz( ). The DH parameters of a serial robot with a certain DOF are shown in Table 4. Table 4. The DH parameters of a serial robot with three degrees-of-freedoms (DOFs). n / d/mm a/mm / 1 d a /2 1 1 1 2 d a /2 2 2 2 3 d a 0 3 3 3 Appl. Sci. 2019, 9, 4365 11 of 36 According to the forward kinematics formula, R = Rotz( )Rotx( )Rotz( )Rotx( )Rotz( )Rotx( ), 1 1 2 2 3 3 (28) R = Rotz( )Roty( )Rotz( ). 1 2 3 A serial robot has three continuous joints meeting = , = and = 0, whose rotational 1 2 3 2 2 components R are known, two sets of solutions can be found using Euler ’s formula. 3.3.1. Three Mutually Perpendicular Sub-Chains Based on in the DH parameters, the conclusions in Section 2.4 can be used to simplify the sub-chains and obtain the expression of the rotational matrix, 2 3 6 cos cos cos + n n sin sin n n sin cos cos cos sin n cos sin 7 s s s s e s e s s s e s e s s s s s s e s s 6 7 1 2 3 1 2 1 3 1 2 1 3 1 2 3 2 1 2 6 7 6 7 6 7 R = 6 sin cos cos n n cos sin n n cos cos sin cos sin n sin sin 7, (29) s s s s e s e s s s e s e s s s s s s e s s 6 1 2 3 1 2 1 3 1 2 1 3 1 2 3 2 1 2 7 6 7 4 5 n sin cos n sin sin n n cos s e s s s e s s s e s e s 1 2 3 1 2 3 1 2 2 for which the following expressions can be obtained: = atan2(n r , n r ) s s e 23 s e 13 1 2 2 > p 2 2 = atan2 r + r ,n n r , (30a) > s s e s e 13 23 33 > 2 1 2 = atan2(n r , n r ) s s e 32 s e 31 3 1 1 or = atan2(n r ,n r ) s s e 23 s e 13 > 1 2 2 2 2 > = atan2 r + r ,n n r , (30b) s 13 23 s e s e 33 > 2 1 2 = atan2(n r ,n r ) s s e s e 32 31 3 1 1 where represents the sum of the rotation angles of the kth sub-chain of the robot and n represents s s e k k the symbol of the last linkage in the kth sub-chain. r represents the element at the position of the xy xth row and the yth column of the rotation matrix R. The problem of solving the sum of the rotation angles of three mutually perpendicular sub-chains is referred to as basic problem 3.3.1. 3.3.2. Two Mutually Perpendicular Sub-Chains Similarly, when there are only two mutually perpendicular sub-chains, the expression of the rotational component at that time is as follows: 2 3 6 cos cos cos sin n sin 7 s s s s s e s 6 1 2 1 2 1 1 7 6 7 6 7 6 7 R = 6 sin cos sin sin n cos 7, (31) s s s s s e s 2 6 1 2 1 2 1 1 7 6 7 4 5 n sin n cos 0 s e s s e s 1 2 1 2 with the corresponding solution: = atan2(n r , n r ) > s s e 31 s e 33 2 1 1 . (32) r r > 21 11 : = atan2 , cos cos s s 2 2 r represents the element at the position of the xth row and the yth column of the rotation matrix xy R. The problem of solving the sum of the rotational degrees of two mutually perpendicular sub-chains is referred to as basic problem 3.3.2. 3.4. The Third Type of Sub-Problem Before the second type of sub-problem is employed by the governing algorithm, the DOF of the robot at that time is i, then the number of DOFs with = 0 in the ﬁrst i 1 DOFs is p. If p , 0, not all Appl. Sci. 2019, 9, 4365 12 of 36 joint angles are obtained by the second type of sub-problem. In this case, the translational component needs to be employed to solve for the remaining joint angles. Such problems are called the third type of sub-problem. For any of this type of sub-problem, when the ﬁrst chain only has one joint, the forward kinematics formula can be directly used to simplify the robotic model. Based on that and according to Equation (4), the translational components of a robot with a DOF of i can be divided into i components. Since the second type of sub-problem can solve the sum of the rotational angles of the sub-chains of the robot, based on Equation (17), the translational equation can be arranged as n=i 0 n1 P P R P R R P = P P R P R R P . (33) s s s s s s n s s s s s s 1 1 2 1 2 3 1 1 2 1 2 3 n=1 P and P represent the rotational and translational components of the kth sub-chain, which is s s k k deﬁned by the formula (11) and formula (12). All terms on the left side of Equation (33) are known and are written as P in the following paragraphs. Conversely, there are unknown quantities on the right side of the equation. Since the third type of problem has a maximum of three sections of sub-chains, [s , s , s ] is used to represent the length of each sub-chain on the right side of the equation. Depending 1 2 3 on the length of each sub-chain, the appropriate formula solutions are presented with s t representing the tth joint in the ith section of the sub-chain. As an example, for z k z ?z k z , there exist two sub-chains, s and s , either of which has a 1 2 3 4 1 2 sub-chain length of two. After the second type of sub-problem is solved, the length of each sub-chain decreases by one. For example, the length of each sub-chain on the right side of Equation (33) is [1, 1, 0]. All possible cases will be analyzed and solved for in the following cases. 3.4.1. Sub-Chain Length as [1,0,0] ˆ ˆ = atan2 a P , a P . s 1 s 1 y s 1 x 1 1 1 h i (34) = . s 2 s s 1 1 1 3.4.2. Sub-Chain Length as [2,0,0] > ˆ > A = P < ˆ B = P , = F (A, B, C), where . s 11 s 12 2 1 1 > 2 2 2 2 ˆ ˆ P +P +a a x y s 1 s 2 : C = 2a s 1 e ˆ P = P a cos x x s 1 s 1 > 1 1 > f ˆ (35) < P = P a sin y y s 1 s 1 e f 1 1 + = F D, L, P , P , where > s 1 s 2 1 x y 1 1 > L = a > s 2 D = 0 h i = f + s 2 s 1 s 2 s 1 1 1 1 1 h i = f + s 3 s s 1 s 2 1 1 1 1 3.4.3. Sub-Chain Length as [2,1,0] and [1,1,0] A = 0 , = F (A, B, C), where B = n a . (36a) s 11 s 12 2 s e s 1 2 2 1 2 C = P d d z s 1 s 2 1 1 h i = f . (36b) s 2 s 1 2 1 1 Appl. Sci. 2019, 9, 4365 13 of 36 P R P = P + R P . (36c) s 1 s 1 s 1 s 2 1 2 1 k k R and P represent the rotational and translational components of the tth joint in the kth section s t s t k k of the sub-chain. According to Equations (36a) and (36b), all joint angles in the sub-chain s can be solved. Moreover, according to Equation (36c), the ﬁrst sub-chain solving problem is transformed into either basic problem 3.6.1 or basic problem 3.6.2. 3.4.4. Sub-Chain Length as [1,2,0] A = n a sin > s s 2 s 2 > 1 1 1 B = n a sin ( ) , = F A, B, C , where > s 2 s 1 s . (37a) s 11 s 12 2 1 1 2 1 1 > : ˆ C = R P d d s s 2 s 1 1 2 2 h i = f . (37b) s 2 s s 1 1 1 R P P = P + R P . (37c) s s 1 s 1 s 1 s 2 1 2 2 2 According to Equations (37a) and (37b), we could solve all joint angles in the sub-chain s . According to Equation (37c), the second sub-chain solving problem can be transformed into basic problem 3.4.2. 3.4.5. Sub-Chain Length as [2,0,1] or [1,0,1] A = 0 , = F (A, B, C), where B = n a sin . (38a) s 11 s 12 2 s e s 1 s 3 3 1 3 3 C = P d d + n n d cos z s 1 s 2 s 3 s 1 s 1 s 1 1 1 2 3 3 h i = f . (38b) s 2 s 1 3 3 3 P R R P = P + R P . (38c) s s s 2 s 2 s 1 s 1 1 2 3 2 2 2 According to Equations (38a) and (38b), all joint angles in the sub-chain s can be solved. According to Equation (38c), the ﬁrst sub-chain solving problem can be transformed into either basic problem 3.4.2 or basic problem 3.4.1. 3.4.6. Sub-Chain Length as [1,0,2] > A = 0 , = F (A, B, C), where B = n a sin . (39a) > s s 21 s 22 2 s 1 s 1 1 1 > 2 1 2 T T C = (R R P) d d + n n d cos s s s 1 s 2 s 2 s 1 s 1 s 2 1 2 z 3 3 1 2 1 h i = f . (39b) s 1 s s 2 1 1 T T T T R R P R R P = P + R P . (39c) s s s s s 1 s 1 s 1 s 2 2 1 2 1 1 3 3 3 According to Equations (39a) and (39b), we could solve all joint angles in the sub-chain s . According to Equation (39c), the third sub-chain solving problem can be transformed into basic problem 3.4.2. Appl. Sci. 2019, 9, 4365 14 of 36 In the case of [1,1,1], it may be impossible to ﬁnd a closed-form solution due to the high complexity of the equation. Therefore, the third type of sub-problem has six basic problems. 3.5. Summary The above-mentioned content proposed the two decoupling methods of series robots ﬁrstly. Three types of sub-problems are proposed through these two decoupling methods. Ten basic problems were derived through analyzing the solvable condition and solution method of each type of sub-problems speciﬁcally. If a series robot can be described through the three types of sub-problem based on these ten basic problems, the robot must be a closed inverse solution. P represents the number of the DOF with = 0 in the ﬁrst i 1 DOFs, i.e., the number of = 0 in the ﬁrst i 1 rows of the DH parameter table. In the case of i P > 3, it means that there are more vertical joints in the robot. There will be more than three unknown numbers that cannot be solved in the rotational component. If there is a closed solution, a low-order trigonometric function equation can only be solved through translation components. There are two basic problems that are feasible. The restrictions and solution formula of the two basic problems is shown in Table 5. Table 5. The restrictions and solution formula of the two basic problems. Base Problem Conﬁguration Restrict Solution Formula 1. The conﬁguration of the ﬁrst g joints is z ?z k ::: k z ?z , 1 2 g1 g Equation (30) can be used to 3.2.1 2. The linkage of z needs to satisfy d = a = 0.. k>g solve 3. The linkage parameters of the last joint are not restricted. Equations (25) and (26) can be 1. The conﬁguration of the ﬁrst three joints is z k z ?z , 1 2 3 used to solve critical coecient, 3.2.2 2. The linkage of z needs to satisfy d = a = 0. k>4 formula (24) can be used to solve 3. The linkage parameters of the last joint are not restricted. the angle of ﬁrst three joints. In the case of i P < 3, it means that there are more parallel joints in the robot. There will be less than three unknown numbers in the rotational component, then, the unknown number can be solved through in the rotational component directly. The parallel joints in the rotation component may be an independent joint angle or the sum of the rotation angles of the sub-chains; therefore, they can be represented as unknown number here. In the case of i P = 3, the formula (30) can be used to solve the three-unknown number. In the case of i P = 2, the formula (32) can be used to solve the two-unknown number. In the case of i P = 1, the ﬁrst formula in the formula (32) can be used to solve the only unknown number. If all joint angles are not solved after using the second sub-problems, it indicates that there is long sub-chain in the translational component. At this time, the remaining joint angles need to be solved by combing the third sub-problems with the result of the second sub-problems. The six basic problems in the third sub-problems are shown in Table 6. Table 6. The six basic problems in the third sub-problems. Base Problem Remaining Length of Each Sub-Chain Solution Formula 3.4.1 [1,0,0] Equation (34) 3.4.2 [2,0,0] Equation (35) 3.4.3 [2,1,0], [1,1,0] Equation (36) and Base problem 3.6.2. 3.4.4 [1,2,0] Equation (37) and Base problem 3.6.2. 3.4.5 [2,0,1], [1,0,1] Equation (38) and Base problem 3.6.2 or 3.6.1. 3.4.6 [1,0,2] Equation (39) and Base problem 3.6.2. Since the third type of problem has a maximum of three sections of sub-chains, [s , s , s ] is used 1 2 3 to represent the length of each sub-chain on the right side of the equation. Depending on the length of Appl. Sci. 2019, 9, 4365 15 of 36 each sub-chain, the appropriate formula solutions are presented with s t representing the tth joint in the kth section of the sub-chain. 4. Algorithm Design for Universal Inverse Kinematics The translational and rotational components of a serial robot with a closed-form solution are mutually decoupled. Therefore, the partial joint angles of the robot can be solved based on the ﬁrst or second type of sub-problem. This simpliﬁes the original robot model into a new robot model. If the new robot model has a closed-form solution, the solution will deﬁnitely continue along one of the three types of sub-problems until all the problems are solved. Therefore, the process of solving the closed-form inverse solution of a serial robot can be completed via constant model simpliﬁcation. Appl. Sci. 2019, 9, x FOR PEER REVIEW 16 of 36 The logical ﬂow chart of the entire algorithm is shown in Figure 2. Start i DOF No Yes i – p > 3 Solving the second Decoupling of types of sub-problems the end linkage simplify the robot Yes No p≠0 Meeting the No Yes Configuration restrict of table 5 End Calculate the remaining length of each subchain The first type of No solution sub-problems Yes Is the case that exists No in Table 6 The third type of No solution sub-problems End Figure 2. Logic for the universal inverse kinematics algorithm. Figure 2. Logic for the universal inverse kinematics algorithm. In In Figure Figure 2,2, P r Pepr represen esents the ts th number e number of of the the DOF w DOF with ith = α=0 0 in the in ﬁrst the if irs 1t DOFs, 𝑖− 1 DO i.e.,Fs, thei.e., number the number of α=0 in the first 𝑖− 1 rows of the DH parameter table. of = 0 in the ﬁrst i 1 rows of the DH parameter table. The inverse kinematics problem has multiple solutions. Nonetheless, the selection of these The inverse kinematics problem has multiple solutions. Nonetheless, the selection of these solutions solutions necessitates a relevant upper logic design. In addition, changes in the robotic joints must be necessitates a relevant upper logic design. In addition, changes in the robotic joints must be continuous: continuous: arg min . (40) (40) 𝑚 i−𝜃 ,cur . i,k , , i=1 In cases where the robot is in a stationary state, the joint angle of the 𝑘 set can be first solved and then Equation (40) can be used to determine the inverse motion index value corresponding to the current position. The main purpose of this paper was to find a more complete inverse solution judgment condition, which is more accurate and specific than the Pieper principle. The general inverse solution algorithm designed based on this judgment condition is also directed to a series robot with a closed inverse solution. For this paper purposed ten basic problems, which means a series of robots with closed inverse solutions can be obtained through combining the basic problems. For example, although 𝑧 ∥𝑧 ⊥𝑧 ∥𝑧 ⊥𝑧 ⊥𝑧 ,or 𝑧 ∥𝑧 ⊥𝑧 ∥𝑧 ∥𝑧 ∥𝑧 are not common, but it is indeed that there are robots with a closed inverse solution. In addition, maintaining the robot configuration does not change, only the direction of rotation and the positive or negative of the offset are changed, the algorithm can still calculate correctly. This can provide great convenience for the application of 𝜃 𝑖𝑛 𝑎𝑟𝑔 Appl. Sci. 2019, 9, 4365 16 of 36 In cases where the robot is in a stationary state, the joint angle of the k set can be ﬁrst solved and then Equation (40) can be used to determine the inverse motion index value corresponding to the current position. The main purpose of this paper was to ﬁnd a more complete inverse solution judgment condition, which is more accurate and speciﬁc than the Pieper principle. The general inverse solution algorithm designed based on this judgment condition is also directed to a series robot with a closed inverse solution. For this paper purposed ten basic problems, which means a series of robots with closed inverse solutions can be obtained through combining the basic problems. For example, although a,d z k z ?z k z ?z ?z , or z k z ?z k z k z k z are not common, but it is indeed that there are robots 1 2 3 6 1 2 3 4 5 6 4 5 with a closed inverse solution. In addition, maintaining the robot conﬁguration does not change, only the direction of rotation and the positive or negative of the oset are changed, the algorithm can still calculate correctly. This can provide great convenience for the application of an engineer. Finally, the logical design of the algorithm is complete. This means that once you use a situation other than ten basic questions to design the robot, the algorithm will jump out and terminate. Therefore, the algorithm is not to solve the inverse solution problem of any robot, but to judge whether the closed inverse solution exists and to solve it. 5. Experiments The previous section discussed the design of the universal inverse kinematics solution algorithm. In this section, four of the experiments will be carried out to verify the completeness, versatility, stability, and real-time performance of the algorithm. Frist of all, the algorithm was implemented in MATLAB 2016b on a 64-bit Windows 10 operating system. The completeness, versatility and stability of the algorithm will be veriﬁed in MATLAB. Subsequently, the algorithm was performed using the Beckho-C6920 controller. We built a six-DOF KingKong robot using the Kollmorgen RGM motor for the real-time testing. 5.1. Veriﬁcation Experiment for Algorithm Completeness Firstly, the algorithm designed in this paper was logically complete. The ﬁrst simulation experiment would test the completeness of the algorithm. Here, a robot with three parallel joints was selected as the experimental object. The DH parameter of it is shown in the Table 7. This robot is a common series robot, but it cannot be described by a single sub-problem. Table 7. The DH parameters of the robot with three parallel joints. n / d/mm a/mm / 1 0.1 0.1 0 2 0.1 0.2 0 3 0.1 0.3 0 First, the algorithm receives the DH parameters of the robot. Algorithm will enter in the second type of sub-problem to solve the rotation angle and of the sub-chain s . The length of the sub-chain s 1 was [2, 0, 0] after been simpliﬁed and calculated, therefore, the remaining joint angles was solved by the basic problem 3.6.2 in the third type sub-problems. 2 3 6 90 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 90 7, and the end posture was: 6 7 6 7 4 5 2 3 0.7071 0.7071 0 0.0121 6 7 6 7 6 7 6 7 6 7 0.7071 0.7071 0 0.1121 6 7 6 7 6 7. (41) 6 7 6 7 0 0 1 0.3 6 7 6 7 4 5 0 0 0 1 Appl. Sci. 2019, 9, 4365 17 of 36 All joint angles were solved with the algorithm, as shown in Table 8. Table 8. Two inversely solved joint angle sets. No. / / / 1 2 3 1 90 90 45 2 36.8699 90 8.1301 It is easy to verify that the two sets of joint angles are correct. A robot with four parallel joints was now used as the experimental object. Its DH parameters are shown in Table 9. Table 9. The DH parameters of the robot with three parallel joints. n / d/m a/m / 1 0.1 0.1 0 2 0.1 0.2 0 3 0.1 0.3 0 4 0.1 0.1 0 The algorithm will enter in the second type of sub-problem the solve the rotation angle and of the sub-chain s . The length of the sub-chain was [3,0,0] after being simpliﬁed and calculated, the algorithm was ended for it was not in the basic problem of the third type sub-problems. In fact, there are only three valid equations for series robots with four-degree-of-freedom, which is impossible to solve its inverse motion problem. Experiment 5.1 veriﬁed the completeness of the algorithm by two examples. The ﬁrst example was a three-parallel joint robot, the correct joint angles were obtained through an algorithm. Then, we used a four-parallel joint robot to test, and the algorithm was terminated for it was not in the basic problems. This shows that the algorithm would exit correctly when it encounters an unsolvable problem, and it will be able to calculate the result when it encounters a solvable problem. This experiment also proved that the proposed judgment method could be used to low-degree- of-freedom robots. In addition, when there were four adjacent parallel joints in a six-degree-of-freedom robot, the Pieper criterion was satisﬁed, but the proposed algorithm could judge that the robot did not have a closed inverse solution. This complements the shortcomings of the Pieper criterion. 5.2. Veriﬁcation Experiment of Algorithm Versatility The proposed algorithm could solve those serial robots that could be described by three types of sub-problems. Since the description of the ten basic problems was determined, a series robot with a closed-form inverse solution could be constructed based on ten basic problems. Two uncommon robots were used as examples to verify the versatility of the algorithm in this experiment. a,d We constructed a robot Bot , whose structure was z k z ?z k z ?z ?z based on basic problem 1 1 2 3 6 4 5 3.4.2 and 3.5.1. Its DH parameters are shown in Table 10. Table 10. The DH parameters of Bot . n / d/m a/m / 1 0.1 0.35 0 2 0.1 0.3 /2 3 0.1 0.5 0 4 0.1 0 /2 5 0 0 /2 6 0.1 0.1 0 6 Appl. Sci. 2019, 9, 4365 18 of 36 The robot had three vertical joints, so the decoupling of the end was carried out ﬁrstly. Then, we a,d found that z k z ?z k z ?z could be solved by the basic problem 3.2.2. When the ﬁrst three joint 1 2 3 4 5 a,d angles were solved, the simpliﬁed robot z ?z ?z could be solved by the second type of sub-problems. 4 5 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end posture was: 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 2 3 0.5625 0.1752 0.8080 0.0228 6 7 6 7 6 7 6 7 6 7 0.8248 0.1875 0.5335 0.6441 6 7 6 7 6 7. (42) 6 7 6 7 0.0580 0.9665 0.2500 0.7192 6 7 6 7 4 5 0 0 0 1 All joint angles were solved with an algorithm, as shown in Table 11. Table 11. Four inversely solved joint angle sets. No. / / / / / / 1 2 3 4 5 6 1 90.0000 60.0000 90.0000 120.0000 30.0000 150.0000 2 90.0000 60.0000 90.0000 60.0000 30.0000 30.0000 3 116.7078 7.3801 90.0000 177.4728 14.4919 89.1752 4 116.7078 7.3801 90.0000 2.5272 14.4919 90.8248 It is easy to verify that the four sets of joint angles are correct. Then, we kept the conﬁguration of the robot unchanged and modiﬁed the positive direction of the rotation of the robot joint, the positive and negative of a and the value of d. This was used to verify whether the algorithm could cope with parameter changes. The changed DH parameters are shown in Table 12. Table 12. The DH parameters of Bot after changing the parameters. n / d/m a/m / 1 0 0.35 0 2 0 0.3 /2 3 0 0.5 0 4 0 0 /2 5 0 0 /2 6 0 0.1 0 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end posture was: 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 2 3 0.5625 0.8248 0.0580 0.2036 6 7 6 7 6 7 6 7 6 7 0.1752 0.1875 0.9665 0.1825 6 7 6 7 . (43) 6 7 6 7 6 7 0.8080 0.5335 0.2500 0.4192 6 7 6 7 4 5 0 0 0 1 All joint angles were computed with the algorithm, as shown in Table 13 Appl. Sci. 2019, 9, 4365 19 of 36 Table 13. Four inversely solved joint angle sets. No. / / / / / / 1 2 3 4 5 6 1 90.0000 60.0000 90.0000 120.0000 30.0000 150.0000 2 90.0000 60.0000 90.0000 60.0000 30.0000 30.0000 3 14.8218 60.0000 90.0000 105.2403 108.0015 51.7522 4 14.8218 60.0000 90.0000 74.7597 108.0015 –128.2478 It is easy to verify that the four sets of joint angles are correct. We constructed the robot Bot , whose structure was z k z ?z k z k z k z based on basic 2 1 2 3 4 5 6 problem 3.5.1 and 3.6.6. Its DH parameters are shown in Table 14. Table 14. The DH parameters of Bot . n / d/m a/m / 1 0.1 0.35 0 2 0.1 0.3 /2 3 0.1 0.3 /2 4 0.1 0.25 0 5 0.1 0.2 0 6 0.1 0.1 0 The robot had three sub-chains, and the second type of sub-problem was ﬁrstly used to obtain the sum of the rotation angle of the three segments. Then, algorithm turned into the third type sub-problems for there were joint angles that had not been solved. In the third sub-problem, we could use the basic problem 3.6.6 to solve, and then solve all joint angles. 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end posture was: 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 2 3 0.0474 0.6597 0.7500 0.9344 6 7 6 7 6 7 6 7 6 7 0.7891 0.4356 0.4330 0.2573 6 7 6 7 . (44) 6 7 6 7 6 7 0.6124 0.6124 0.5000 0.0017 6 7 6 7 4 5 0 0 0 1 All joint angles were solved with the algorithm, as shown in Table 15. Table 15. Four inversely solved joint angle sets. No. / / / / / / 2 3 5 6 1 4 1 150.0000 60.0000 60.0000 95.1093 100.7234 129.3859 2 150.0000 60.0000 60.0000 9.6646 100.7234 114.6120 3 90.0000 60.0000 60.0000 45.0000 45.0000 45.0000 4 90.0000 60.0000 60.0000 84.7298 45.0000 95.2702 It is easy to verify that the four sets of joint angles are correct. In this group of experiments, the test subjects were two uncommon robots constructed by basic questions. However, according to the proposed theory, these two robots had a closed inverse solution. In the experiment, the inverse solution of both robots was solved correctly. In addition, for the ﬁrst robot, its conﬁguration remains unchanged, but the link parameters were greatly modiﬁed, the correct Appl. Sci. 2019, 9, 4365 20 of 36 result was obtained in the end. It could be seen that the algorithm had good versatility. As long as a given robot can be described by three sub-problems, its closed-from solution can be obtained whether it is in low degree of freedom or six degrees of freedom. At the same time, for the robot with the same conﬁguration, the change of the link parameters does not aect the inverse kinematics solution. 5.3. Veriﬁcation Experiment for the Algorithm Continuity This experiment would use the common Puma560 as the subject. In the experiment, the robot should move according to the speciﬁed trajectory. We needed to observe whether the joint angle after the inverse solution was continuous when solving the continuous space trajectory. This is important in the actual movement of robot. If there is a jump or discontinuity in the joint space curve, it will cause great impact to the motor, and the curve of the end also does not move according to the speciﬁed trajectory. The DH parameters of the Puma560 robot are shown in Table 16. Table 16. The DH parameters of the Puma560 robot. n / d/mm a/mm / 1 0 0 /2 2 0 43.18 0 3 150.03 20.3 /2 4 43.18 0 /2 5 0 0 /2 6 0 0 0 The quantity of the vertical joints was judged following the input of the algorithm into the DH model of the Puma560 robot. Subsequently, the algorithm entered the ﬁrst type of sub-problem. Then, joint decoupling was performed for J and J ?J k J ?J ?J . Based on basic problem 3.4.1, 6 1 2 3 4,a 5,a,d the angle value of was obtained. In addition, after was obtained, T was obtained using to 1 1 the forward kinematics formula. After it was simpliﬁed, J k J ?J ?J ?J formed a new robot 2 3 4,a 6 5,a,d model and was substituted into the algorithm. Similarly, J k J ?J ?J was solved using basic 2 3 4,a 5,a,d problem 3.2.2 and and were subsequently obtained. After the reduction in the DH parameters 2 3 and the transformational matrices, T and the robot model J ?J ?J were substituted into the 4,a 5,a,d 6 algorithm to solve the rational component in the second type of sub-problem. All joint angles were solved at that time. 2 3 25.5667 6 7 6 7 6 7 6 7 6 7 0.0624 6 7 6 7 6 7 6 7 6 7 3.0736 6 7 6 7 Consequently, the initial position was Q = 6 7, and the end execution posture was: 6 7 6 7 25.5975 6 7 6 7 6 7 6 7 6 7 87.2840 6 7 6 7 4 5 1.3005 2 3 0 0 1 0.4521 6 7 6 7 6 7 6 7 6 7 0 1 0 0.0499 6 7 6 7 . (45) 6 7 6 7 6 7 1 0 0 0.4318 6 7 6 7 4 5 0 0 0 1 All joint angles were computed with the algorithm, as shown in Table 17. Here we also used the kinematic inverse function in the Robotics Toolbox, and could also ﬁnd eight groups of solutions, as shown in Table 18. It could be seen that part of the joint angles have exceeded h i the scope of . The reason is that there was no uniform inverse trigonometric function selected in the Robotics Toolbox. In actual use, for example, the Beckho-C6920 controller and the Kollmorgen RGM motor, both use real numbers to describe the motion of the motor rather than a positive real number. Therefore, the method proposed in this paper had certain advantages from this point of view. Appl. Sci. 2019, 9, 4365 21 of 36 Table 17. Eight inversely solved joint angle sets. No. / / / / / / 1 2 3 4 5 6 1 167.0434 89.6199 3.0711 78.4733 166.7756 101.8339 2 167.0434 89.6199 3.0711 101.5267 166.7756 78.1661 3 167.0434 179.9370 177.6878 167.0362 92.3124 0.5341 4 167.0434 179.9370 177.6878 12.9638 92.3124 179.4659 5 25.5654 0.0630 3.0711 25.5998 87.2844 1.3006 6 25.5654 0.0630 3.0711 154.4002 87.2844 178.6994 7 25.5654 90.3801 177.6878 84.3875 154.2989 83.7756 8 25.5654 90.3801 177.6878 95.6125 154.2989 96.2244 Appl. Sci. 2019, 9, x FOR PEER REVIEW 22 of 36 Table 18. Eight inversely solved joint angle sets by the Robotics Toolbox. Table 18. Eight inversely solved joint angle sets by the Robotics Toolbox. No. / / / / / / 1 2 3 4 5 6 1 167.0434 89.6199 3.0711 78.4733 166.7756 101.8339 No. 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝟏 𝟐 𝟑 𝟒 𝟓 𝟔 2 167.0434 89.6199 3.0711 101.5267 166.7756 78.1661 1 167.0434 89.6199 3.0711 -78.4733 166.7756 101.8339 3 167.0434 180.0624 182.3097 167.0320 92.3124 0.5322 2 167.0434 89.6199 3.0711 101.5267 -166.7756 -78.1661 4 167.0434 180.0624 182.3097 12.9638 92.3124 179.4659 3 167.0434 180.0624 182.3097 -167.0320 92.3124 0.5322 5 25.5654 90.3801 182.3097 84.3875 154.2989 83.7756 6 25.5654 90.3801 182.3097 95.6086 154.2989 96.2197 4 167.0434 180.0624 182.3097 12.9638 -92.3124 -179.4659 7 25.5654 0.0624 3.0736 25.5975 87.2840 1.3005 5 25.5654 90.3801 182.3097 -84.3875 154.2989 -83.7756 8 25.5654 0.0624 3.0736 154.4025 87.2840 178.6995 6 25.5654 90.3801 182.3097 95.6086 -154.2989 96.2197 7 25.5654 -0.0624 3.0736 -25.5975 87.2840 1.3005 The robot was instructed to move as per the Equation (51) spiral line with a step size of 0.01: 8 25.5654 0.0624 3.0736 154.4025 -87.2840 -178.6995 > x = 0.3t The robot was instructed to mov >e as per the Equation (51) spiral line with a step size of 0.01: y = 0.2cos(2t) 0.2 , t 2 [0, 1]. (46) 𝑥 = > 0.3𝑡 (46) 𝑦= 0.2 𝑐𝑜𝑠 (2𝜋𝑡z = ) −0 0.2sin .2 (2t) ,𝑡 ∈ [0,1]. 𝑧= 0.2𝑖𝑛𝑠(2𝜋𝑡) According to formula (40), with the ﬁfth set of solutions selected. According to the joint angle, According to formula (40), with the fifth set of solutions selected. According to the joint angle, the end position was recomputed with the motion trajectory shown in Figure 3. The joint angle was the end position was recomputed with the motion trajectory shown in Figure 3. The joint angle was inversely solved as per the end trajectory to obtain the continuous joint position, as shown in Figure 4. inversely solved as per the end trajectory to obtain the continuous joint position, as shown in Figure Figure 3. The motion trajectory of the Puma560 robot. Figure 3. The motion trajectory of the Puma560 robot. As shown in Table 17, the algorithm could solve for multiple sets of joint angles, and it could be conﬁrmed via positive kinematics that all the joint angles were correct. In addition, in the planning experiment of the Cartesian space trajectory, the joint angle inversely solved according to the target trajectory is shown in Figure 4 and its curve changed continuously without jumps. In Figure 3, the end Figure 4. The joint trajectory after the inverse kinematics solution. Appl. Sci. 2019, 9, x FOR PEER REVIEW 22 of 36 Table 18. Eight inversely solved joint angle sets by the Robotics Toolbox. No. 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝟏 𝟐 𝟑 𝟒 𝟓 𝟔 1 167.0434 89.6199 3.0711 -78.4733 166.7756 101.8339 2 167.0434 89.6199 3.0711 101.5267 -166.7756 -78.1661 3 167.0434 180.0624 182.3097 -167.0320 92.3124 0.5322 4 167.0434 180.0624 182.3097 12.9638 -92.3124 -179.4659 5 25.5654 90.3801 182.3097 -84.3875 154.2989 -83.7756 6 25.5654 90.3801 182.3097 95.6086 -154.2989 96.2197 7 25.5654 -0.0624 3.0736 -25.5975 87.2840 1.3005 8 25.5654 0.0624 3.0736 154.4025 -87.2840 -178.6995 The robot was instructed to move as per the Equation (51) spiral line with a step size of 0.01: 𝑥 = 0.3𝑡 (46) 𝑦= 0.2 𝑐𝑜𝑠 (2𝜋𝑡 ) −0.2 [ ] ,𝑡 ∈ 0,1 . 𝑧= 0.2𝑖𝑛𝑠(2𝜋𝑡) According to formula (40), with the fifth set of solutions selected. According to the joint angle, the end position was recomputed with the motion trajectory shown in Figure 3. The joint angle was inversely solved as per the end trajectory to obtain the continuous joint position, as shown in Figure Appl. Sci. 2019, 9, 4365 22 of 36 trajectory was recalculated from the obtained joint angle and was consistent with the planned trajectory. Figure 3. The motion trajectory of the Puma560 robot. This proved that the algorithm could correctly solve the inverse kinematics of the spatial trajectory. Appl. Sci. 2019, 9, x FOR PEER REVIEW 23 of 36 As shown in Table 17, the algorithm could solve for multiple sets of joint angles, and it could be confirmed via positive kinematics that all the joint angles were correct. In addition, in the planning experiment of the Cartesian space trajectory, the joint angle inversely solved according to the target trajectory is shown in Figure 4 and its curve changed continuously without jumps. In Figure 3, the end trajectory was recalculated from the obtained joint angle and was consistent with the planned trajectory. This proved that the algorithm could correctly solve the inverse kinematics of the spatial trajectory. 5.4. Real-Time Verification Experiment of the Algorithm Figure 4. The joint trajectory after the inverse kinematics solution. Figure 4. The joint trajectory after the inverse kinematics solution. In the industry, the robot controller sends position information to the motor driver in a certain 5.4. Real-Time Veriﬁcation Experiment of the Algorithm cycle, the shorter the cycle, the better the real-time performance. For some occasions with high In the industry, the robot controller sends position information to the motor driver in a certain precision, if the cycle is too long, a big error will occur in the outline of the end. In addition, if the cycle, the shorter the cycle, the better the real-time performance. For some occasions with high precision, desired end trajectory is constantly changing, the controller is also required to be able to react and if the cycle is too long, a big error will occur in the outline of the end. In addition, if the desired end plan quickly during the cycle. Therefore, real-time performance is an important performance trajectory is constantly changing, the controller is also required to be able to react and plan quickly indicator in the robot controlling field. In order to improve the closed-loop period of the system, in during the cycle. Therefore, real-time performance is an important performance indicator in the robot addition to the purchase of more powerful hardware devices, the more important factor is the controlling field. In order to improve the closed-loop period of the system, in addition to the purchase operation speed of algorithm. Therefore, this paper designed a verification experiment whose real- of more powerful hardware devices, the more important factor is the operation speed of algorithm. time performance was closed to prove that the algorithm proposed in this paper was not only Therefore, this paper designed a verification experiment whose real-time performance was closed to accurate but also efficient and fast. prove that the algorithm proposed in this paper was not only accurate but also efficient and fast. The six-DOF KingKong robot built with a Beckhoff-C6920 controller and a Kollmorgen RGM The six-DOF KingKong robot built with a Beckho-C6920 controller and a Kollmorgen RGM motor was used in the experiment testing the correctness and real-time performance of the algorithm. motor was used in the experiment testing the correctness and real-time performance of the algorithm. The DH parameters of the robot are listed in Table 3. The C6920 controller in Figure 5. The C6920 The DH parameters of the robot are listed in Table 3. The C6920 controller in Figure 5. The C6920 controller used a 32-bit Windows 7 operating system and was equipped with an Intel Core i5 controller used a 32-bit Windows 7 operating system and was equipped with an Intel Core i5 processor. processor. Figure Figure 5. 5. The C6920 The C6920 controller. controller. Figure 6 displays the control framework for the entire system. When the controller received the Figure 6 displays the control framework for the entire system. When the controller received the order of motion, the critical parameters of the locus equation were computed according to the order. order of motion, the critical parameters of the locus equation were computed according to the order. Subsequently, the position point at the next time point was planned in the Cartesian spatial planner. Based on the position points in Cartesian space, the inverse kinematics solving method proposed here was employed to obtain the position to be reached by each motor at the next time point. The closed-loop circle of the motor controller was 2 ms. The controller sent the motion order to the RGM motor via CanOpen. CanOpen is a high-level communication protocol based on the Controller Area Network. It is often used in embedded systems and is a type of field bus commonly used in industrial control. Appl. Sci. 2019, 9, 4365 23 of 36 Subsequently, the position point at the next time point was planned in the Cartesian spatial planner. Based on the position points in Cartesian space, the inverse kinematics solving method proposed here was employed to obtain the position to be reached by each motor at the next time point. The closed-loop Appl. Sci. 2019, 9, x FOR PEER REVIEW 24 of 36 circle of the motor controller was 2 ms. The controller sent the motion order to the RGM motor via CanOpen. CanOpen is a high-level communication protocol based on the Controller Area Network. It is often used in embedded systems and is a type of ﬁeld bus commonly used in industrial control. Appl. Sci. 2019, 9, x FOR PEER REVIEW 24 of 36 Cartesian space planner M M M RGM14 RGM14 RGM14 Cartesian space planner Ikine Ikine M M M RGM14 RGM14 RGM14 CanOpen M M M Ikine Ikine Motion RGM20 RGM25 RGM25 CanOpen M M Beckhoff-C6920 Motion RGM20 RGM25 RGM25 Figure 6. Control structure of the system. Beckhoff-C6920 The number of vertical joints was judged according to the KingKong DH model. Then, the Figure 6. Control structure of the system. Figure 6. Control structure of the system. algorithm assessed the first type of sub-problem. After joint decoupling was performed for 𝐽 , 𝐽 ⊥ 𝐽 ∥𝐽 The ∥𝐽 number ⊥𝐽 wa of vertical s able to joints solve was for judged 𝜃 uaccor sing b ding asicto pthe robKingKong lem 3.2.1. A DH ftmodel. er 𝜃 wa Then, s ob the tain algorithm ed, 𝑇 and The number of vertical joints was judged according to the KingKong DH model. Then, the assessed the ﬁrst type of sub-problem. After joint decoupling was performed for J , J ?J k J k J ?J 𝐽 ∥𝐽 ∥𝐽 ⊥𝐽 ⊥𝐽 were obtained using the forward kinematics formula. At t 6 he time, t 2 3 he 5, robot a 1 4 algorithm assessed the first type of sub-problem. After joint decoupling was performed for 𝐽 , 𝐽 ⊥ was able to solve for using basic problem 3.2.1. After was obtained, T and J k J k J ?J ?J belonged to the second type of sub-pr 1 oblem and we could d 1 irectly solve for 𝜃 2+𝜃 3 +𝜃 4 ,𝜃 5,a and 6 𝜃 . 𝐽 ∥𝐽 ∥𝐽 ⊥𝐽 was able to solve for 𝜃 using basic problem 3.2.1. After 𝜃 was obtained, 𝑇 and were obtained using the forward kinematics formula. At the time, the robot belonged to the second Subsequently, basic problem 3.4.2 was used to solve for 𝜃 and 𝜃 +𝜃 and to obtain 𝜃 and 𝜃 . 𝐽 ∥𝐽 ∥𝐽 ⊥𝐽 ⊥𝐽 were obtained using the forward kinematics formula. At the time, the robot type of sub-problem and we could directly solve for + + , and . Subsequently, basic 2 3 4 5 6 In this experiment, the robot would be commanded to move according to the specified spatial belonged to the second type of sub-problem and we could directly solve for 𝜃 +𝜃 +𝜃 ,𝜃 and 𝜃 . problem 3.4.2 was used to solve for and + and to obtain and . 2 2 3 3 trajectory in the real environment, and the acceleration process of the motor must be considered. A Subsequently, basic problem 3.4.2 was used to solve for 𝜃 and 𝜃 +𝜃 and to obtain 𝜃 and 𝜃 . In this experiment, the robot would be commanded to move according to the speciﬁed spatial common acceleration planning method is a seven-segment S curve. This method uses the square In this experiment, the robot would be commanded to move according to the specified spatial trajectory in the real environment, and the acceleration process of the motor must be considered. wave Jerk curve as the input of the system, and the acceleration, velocity, and position quantities trajectory in the real environment, and the acceleration process of the motor must be considered. A A common acceleration planning method is a seven-segment S curve. This method uses the square obtained through the integrating of time, as is shown Figure 7. common acceleration planning method is a seven-segment S curve. This method uses the square wave Jerk curve as the input of the system, and the acceleration, velocity, and position quantities wave Jerk curve as the input of the system, and the acceleration, velocity, and position quantities obtained through the integrating of time, as is shown Figure 7. obtained through the integrating of time, as is shown Figure 7. (a) (a) (b) (b) (c) (c) (d) (d) Figure 7. (a) Displacement of the seven-segment S curve over time. (b) Velocity of the seven-segment S Figure 7. (a) Displacement of the seven-segment S curve over time. (b) Velocity of the seven-segment curve over time. (c) Acceleration of the seven-segment S curve over time. (d) Jerk of seven-segment S S curve over time. (c) Acceleration of the seven-segment S curve over time. (d) Jerk of seven-segment curve over time. S curve over time. Figure 7. (a) Displacement of the seven-segment S curve over time. (b) Velocity of the seven-segment S curve over time. (c) Acceleration of the seven-segment S curve over time. (d) Jerk of seven-segment It can be seen from the figure that the acceleration curve of the seven-segment S-curve S curve over time. constructed by square wave Jerk input is a linear piecewise function. Although it is continuous, but there are a finite number of non-conductible breakpoints. If the acceleration can be made to have It can be seen from the figure that the acceleration curve of the seven-segment S-curve higher order differentiable properties, the robot movement can be smoother. constructed by square wave Jerk input is a linear piecewise function. Although it is continuous, but In the Descartes spatial planner of the controller, the 15-segment S-curve spatial trajectory there are a finite number of non-conductible breakpoints. If the acceleration can be made to have planning method was used. When the objective position, S, and the maximum values of the curve, higher order differentiable properties, the robot movement can be smoother. 𝑆 ,𝑆 ,and 𝑆 , are given, this method can provide a smooth curve and the speed utilization rate can In the Descartes spatial planner of the controller, the 15-segment S-curve spatial trajectory approximate the optimal solution. In 15-segment S planning, the jerk input uses the sine piecewise planning method was used. When the objective position, S, and the maximum values of the curve, 𝑆 ,𝑆 ,and 𝑆 , are given, this method can provide a smooth curve and the speed utilization rate can approximate the optimal solution. In 15-segment S planning, the jerk input uses the sine piecewise Appl. Sci. 2019, 9, x FOR PEER REVIEW 25 of 36 function Equation (52) to replace the jerk square wave input in the traditional seven-segment S curve. A schematic diagram for the 15-segment S curve is shown in Figure 8. Appl. Sci. 2019, 9, 4365 24 of 36 Compared with the traditional seven-segment S curve, the 15-segment curve proposed in this paper was constructed by sinusoidal function. We know that sinusoidal functions have infinite It can be seen from the ﬁgure that the acceleration curve of the seven-segment S-curve constructed differentiable properties. Therefore, the acceleration curve of the 15-segment S curve also has infinite by square wave Jerk input is a linear piecewise function. Although it is continuous, but there are a differentiable properties. The curve obtained by this planning method is smoother. ﬁnite number of non-conductible breakpoints. If the acceleration can be made to have higher order dierentiable properties, the robot movement can be smoother. 𝑠𝑖𝑛 𝑛𝑡 𝑥 ∈ [0, ] In the Descartes ⎪ spatial planner of the controller, the 15-segment S-curve spatial trajectory planning . .. ... (47) ℎ (𝑡 ) = 1𝑥∈[ , +𝑚] . method was used. When the objective position, S, and the maximum values of the curve, S, S, and , 𝑐𝑜𝑠 𝑛(𝑡 − 𝑚) 𝑥 ∈ [ +𝑚, +𝑚] are given, this method can provide a smooth curve and the speed utilization rate can approximate the optimal solution. In 15-segment S planning, the jerk input uses the sine piecewise function Equation (52) In Equation (47), 𝑛 is an adjustable parameter to adjust the rising speed of the jerk curve and 𝑚 to replace the jerk square wave input in the traditional seven-segment S curve. A schematic diagram for the 15-segment S curve is shown in Figure 8. depends on the maximum values of 𝑆 and 𝑆 . (a) (b) (c) (d) Figure 8. (a) Displacement of the 15-segment S curve over time. (b) Velocity of the 15-segment S curve Figure 8. (a) Displacement of the 15-segment S curve over time. (b) Velocity of the 15-segment S curve over time. (c) Acceleration of the 15-segment S curve over time. (d) Jerk of 15-segment S curve over time. over time. (c) Acceleration of the 15-segment S curve over time. (d) Jerk of 15-segment S curve over Compared with the traditional seven-segment S curve, the 15-segment curve proposed in this time. paper was constructed by sinusoidal function. We know that sinusoidal functions have inﬁnite dierentiable properties. Therefore, the acceleration curve of the 15-segment S curve also has inﬁnite 0° ⎡ ⎤ −60° dierentiable properties. The curve obtained by this planning method is smoother. ⎢ ⎥ 120° 8 h ⎢i ⎥ The initial position of the KingKong robot was Θ= , as shown in Figure 9. > 1 > −135° sin nt x 2 0, ⎢ ⎥ 2 n > h i ⎢ −45° ⎥ 1 1 h (t) = 1 x 2 , + m . (47) n > n n > ⎣ ⎦ h i −45° 1 2 cos n(t m) x 2 + m, + m 2 n n In Equation (47), n is an adjustable parameter to adjust the rising speed of the jerk curve and m .. ... depends on the maximum values of S and S . 2 3 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 The initial position of the KingKong robot was Q = 6 7, as shown in Figure 9. 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 6 7 4 5 (a) ( b) Figure 9. (a) Robot in its initial position from the front. (b) Robot in its initial position from the side. At that time, based on the forward kinematics computation, the end posture was: Appl. Sci. 2019, 9, x FOR PEER REVIEW 25 of 36 function Equation (52) to replace the jerk square wave input in the traditional seven-segment S curve. A schematic diagram for the 15-segment S curve is shown in Figure 8. Compared with the traditional seven-segment S curve, the 15-segment curve proposed in this paper was constructed by sinusoidal function. We know that sinusoidal functions have infinite differentiable properties. Therefore, the acceleration curve of the 15-segment S curve also has infinite differentiable properties. The curve obtained by this planning method is smoother. 𝑠𝑖𝑛 𝑛𝑡 𝑥 ∈ [0, ] (47) ℎ (𝑡 ) = 1𝑥∈[ , +𝑚] . 𝑐𝑜𝑠 𝑛(𝑡 − 𝑚) 𝑥 ∈ [ +𝑚, +𝑚] In Equation (47), 𝑛 is an adjustable parameter to adjust the rising speed of the jerk curve and 𝑚 depends on the maximum values of 𝑆 and 𝑆 . (a) (b) (c) (d) Figure 8. (a) Displacement of the 15-segment S curve over time. (b) Velocity of the 15-segment S curve over time. (c) Acceleration of the 15-segment S curve over time. (d) Jerk of 15-segment S curve over time. 0° ⎡ ⎤ −60° ⎢ ⎥ 120° ⎢ ⎥ The initial position of the KingKong robot was Θ= , as shown in Figure 9. −135° ⎢ ⎥ ⎢ ⎥ Appl. Sci. 2019, 9, 4365 −45° 25 of 36 ⎣ ⎦ −45° (a) (b) Figure 9. (a) Robot in its initial position from the front. (b) Robot in its initial position from the side. Figure 9. (a) Robot in its initial position from the front. (b) Robot in its initial position from the side. At that time, based on the forward kinematics computation, the end posture was: At that time, based on the forward kinematics computation, the end posture was: 2 3 0.5536 0.8124 0.1830 0.4961 6 7 6 7 6 7 6 7 6 7 0.5000 0.5000 0.7071 0.1330 6 7 6 7 6 7. (48) 6 7 6 7 0.6660 0.3000 0.6830 0.0895 6 7 6 7 4 5 0 0 0 1 The algorithm was used to obtain the inverse solution and eight sets of joint angles, as shown in Table 19. According to formula (40) the fourth set of solutions was selected and applied in the motion control logic. Table 19. Eight sets of reversely solved joint angles. No. / / / / / / 1 2 3 4 5 6 1 0 38.3143 80.0936 146.7793 45.0000 135.0000 2 0 35.7525 80.0936 60.6588 45.0000 135.0000 3 0 47.6172 120.0000 2.6172 45.0000 45.0000 4 0 60.0000 120.0000 135.0000 45.0000 45.0000 5 159.2347 125.5572 120.8932 16.9043 136.5570 15.1304 6 159.2347 126.1566 120.8932 150.4045 136.5570 15.1304 7 159.2347 140.7508 79.3110 136.7071 136.5570 164.8696 8 159.2347 145.8820 79.3110 51.4522 136.5570 164.8696 The spatial planning was as follows. The experiment used a spiral line as the expected end trajectory. The parameter equation of the end trajectory spiral line is: > x = 0.15 cos(2t) 0.15 > y = 0.15 sin(t) , t 2 [0, 5] . (49) z = 0.05t First, according to Equation (50), the arc length of the spiral line is: ! ! ! 2 2 2 dy dx dz l = + + dt = 2.3694 m. (50) dt dt dt . .. ... Given S = 0.125 m/s, S = 0.025 m/s, = 0.01 m/s, and n = 1, the 15-segment S curve max max S max was used to plan the end arc length. The total time consumed was 28.1821 s. The variation curve at each stage of the trajectory is shown in Figure 10. Appl. Sci. 2019, 9, x FOR PEER REVIEW 26 of 36 −0.5536 0.8124 0.1830 −0.4961 0.5000 0.5000 −0.7071 −0.1330 (48) −0.6660 −0.3000 −0.6830 0.0895 The algorithm was used to obtain the inverse solution and eight sets of joint angles, as shown in Table 19. According to formula (40) the fourth set of solutions was selected and applied in the motion control logic. Table 19. Eight sets of reversely solved joint angles. No. 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝜽 /° 𝟏 𝟐 𝟑 𝟒 𝟓 𝟔 1 0 38.3143 -80.0936 146.7793 45.0000 135.0000 2 0 -35.7525 80.0936 60.6588 45.0000 135.0000 3 0 47.6172 -120.0000 -2.6172 -45.0000 -45.0000 4 0 -60.0000 120.0000 -135.0000 -45.0000 -45.0000 5 -159.2347 -125.5572 -120.8932 -16.9043 136.5570 -15.1304 6 -159.2347 126.1566 120.8932 -150.4045 136.5570 -15.1304 7 -159.2347 -140.7508 -79.3110 136.7071 -136.5570 164.8696 8 -159.2347 145.8820 79.3110 51.4522 -136.5570 164.8696 The spatial planning was as follows. The experiment used a spiral line as the expected end trajectory. The parameter equation of the end trajectory spiral line is: ( ) 𝑥 = 0.15 cos 2𝜋𝑡 − 0.15 (49) , 𝑡 ∈ [0,5]. 𝑦 = 0.15 sin (𝜋𝑡 ) 𝑧 = 0.05𝑡 First, according to Equation (50), the arc length of the spiral line is: 𝑑𝑥 𝑑𝑦 𝑑𝑧 (50) ℓ= ( ) +( ) +( ) = 2.3694 m. 𝑑𝑡 𝑑𝑡 𝑑𝑡 Given 𝑆 = 0.125 m/s， 𝑆 = 0.025 m/s， 𝑆 =0.01 m/s, and 𝑛= 1 , the 15-segment S curve was used to plan the end arc length. The total time consumed was 28.1821 s. The variation curve at each stage of the trajectory is shown in Figure 10. Appl. Sci. 2019, 9, 4365 26 of 36 Appl. Sci. 2019, 9, x FOR PEER REVIEW 27 of 36 Figure 10. (a) Displacement of the spatial trajectory over time. (b) Velocity of the spatial trajectory over time. (c) Acceleration of the spatial trajectory over time. (d) Jerk of the spatial trajectory over (a) (b) (c) (d) time. Figure 10. (a) Displacement of the spatial trajectory over time. (b) Velocity of the spatial trajectory over The arc length obtained according to Equation (50) was used to rewrite the parametric equation: time. (c) Acceleration of the spatial trajectory over time. (d) Jerk of the spatial trajectory over time. ⎧𝑥 = 0.15 ( ) − 0.15 The arc length obtained according to Equation (50) was used to rewrite the parametric equation: ⎪ 0.1581 8 ,𝑙 ∈ [0, ℓ]. (51) > x = 0.15 cos 0.15 ⎨ 𝑦 = 0.15 𝑠𝑖𝑛( ) > 0.1581 < 0.1581 , l 2 [0,l] . (51) > y = 0.15 sin ⎩ > 0.1581 > 𝑧 = 0.1581 𝑙 z = 0.1581 l In summary, the position curve in Cartesian space and its various derivatives could be obtained, In summary, the position curve in Cartesian space and its various derivatives could be obtained, together with the planned position curves of each joint and their various derivatives, as shown in together with the planned position curves of each joint and their various derivatives, as shown in Figures 11–16. Figures 11–16. (a) (b) (c) (d) Figure 11. (a) Displacement of axis I over time. (b) Velocity of axis I over time. (c) Acceleration of axis I Figure 11. (a) Displacement of axis I over time. (b) Velocity of axis I over time. (c) Acceleration of axis over time. (d) Jerk of axis I over time. I over time. (d) Jerk of axis I over time. (a) (b) (c) (d) 𝑐𝑜𝑠 𝑑𝑡 Appl. Sci. 2019, 9, x FOR PEER REVIEW 27 of 36 Figure 10. (a) Displacement of the spatial trajectory over time. (b) Velocity of the spatial trajectory over time. (c) Acceleration of the spatial trajectory over time. (d) Jerk of the spatial trajectory over time. The arc length obtained according to Equation (50) was used to rewrite the parametric equation: 𝑥 = 0.15 ( ) − 0.15 0.1581 (51) 𝑙 ,𝑙 ∈ [0, ℓ]. ⎨ 𝑦 = 0.15 𝑠𝑖𝑛( ) 0.1581 𝑧 = 0.1581 𝑙 In summary, the position curve in Cartesian space and its various derivatives could be obtained, together with the planned position curves of each joint and their various derivatives, as shown in Figures 11–16. (a) (b) (c) (d) Figure 11. (a) Displacement of axis I over time. (b) Velocity of axis I over time. (c) Acceleration of axis I over time. (d) Jerk of axis I over time. Appl. Sci. 2019, 9, 4365 27 of 36 Appl. Sci. 2019, 9, x FOR PEER REVIEW 28 of 36 Figure 12. (a) Displacement of axis II over time. (b) Velocity of axis II over time. (c) Acceleration of (a) (b) (c) (d) axis II over time. (d) Jerk of axis II over time. Figure 12. (a) Displacement of axis II over time. (b) Velocity of axis II over time. (c) Acceleration of axis II over time. (d) Jerk of axis II over time. (a) (b) (c) (d) Figure 13. (a) Displacement of axis III over time. (b) Velocity of axis III over time. (c) Acceleration of Figure 13. (a) Displacement of axis III over time. (b) Velocity of axis III over time. (c) Acceleration of axis III over time. (d) Jerk of axis III over time. axis III over time. (d) Jerk of axis III over time. (a) (b) (c) (d) Figure 14. (a) Displacement of axis IV over time. (b) Velocity of axis IV over time. (c) Acceleration of axis IV over time. (d) Jerk of axis IV over time. 𝑐𝑜𝑠 Appl. Sci. 2019, 9, x FOR PEER REVIEW 28 of 36 Figure 12. (a) Displacement of axis II over time. (b) Velocity of axis II over time. (c) Acceleration of axis II over time. (d) Jerk of axis II over time. (a) (b) (c) (d) Figure 13. (a) Displacement of axis III over time. (b) Velocity of axis III over time. (c) Acceleration of axis III over time. (d) Jerk of axis III over time. Appl. Sci. 2019, 9, 4365 28 of 36 Appl. Sci. 2019, 9, x FOR P (a)EER REVIEW (b) (c) (d) 29 of 36 Figure 14. (a) Displacement of axis IV over time. (b) Velocity of axis IV over time. (c) Acceleration of Figure 14. (a) Displacement of axis IV over time. (b) Velocity of axis IV over time. (c) Acceleration of axis IV over time. (d) Jerk of axis IV over time. axis IV over time. (d) Jerk of axis IV over time. (a) (b) (c) (d) Figure 15. Figure (a 15. ) Displacem (a) Displacement ent of ax ofis axis V over V over time. time. (b (b ) Veloc ) Velocity ity of of ax axis isV V ov overetime. r time (. c ( ) c Acceleration ) Acceleration of of axis V over time. (d) Jerk of axis V over time. axis V over time. (d) Jerk of axis V over time. (a) (b) (c) (d) Figure 16. (a) Displacement of axis VI over time. (b) Velocity of axis VI over time. (c) Acceleration of axis VI over time. (d) Jerk of axis VI over time. Figure 17 shows the position after the robot has finished its run. The curve of the actual motion of the robot in Cartesian space was calculated from the forward kinematics and is shown together with the planned spatial curve in Figure 18. After the run was completed, the actual motion curve of each joint was acquired from the controller, as shown in Figure 19. Appl. Sci. 2019, 9, x FOR PEER REVIEW 29 of 36 (a) (b) (c) (d) Figure 15. (a) Displacement of axis V over time. (b) Velocity of axis V over time. (c) Acceleration of axis V over time. (d) Jerk of axis V over time. Appl. Sci. 2019, 9, 4365 29 of 36 (a) (b) (c) (d) Figure 16. (a) Displacement of axis VI over time. (b) Velocity of axis VI over time. (c) Acceleration of Figure 16. (a) Displacement of axis VI over time. (b) Velocity of axis VI over time. (c) Acceleration of axis VI over time. (d) Jerk of axis VI over time. axis VI over time. (d) Jerk of axis VI over time. Figure 17 shows the position after the robot has ﬁnished its run. The curve of the actual motion of Figure 17 shows the position after the robot has finished its run. The curve of the actual motion the robot in Cartesian space was calculated from the forward kinematics and is shown together with of the robot in Cartesian space was calculated from the forward kinematics and is shown together the planned spatial curve in Figure 18. After the run was completed, the actual motion curve of each Appl. Appl. Sci. Sci. 2019 2019 , 9 , ,9 x FO , x FO R P R P EER EER RE RE VIEW VIEW 30 of 30 of 36 36 with the planned spatial curve in Figure 18. After the run was completed, the actual motion curve of joint was acquired from the controller, as shown in Figure 19. each joint was acquired from the controller, as shown in Figure 19. (a () ( a) ( bb ) ) Figure 17. Figure Figure 17. 17. (a( () a aPosture of the ) ) Posture of the Posture of the ro ro rbot after obot bot after after completion of completion completion of of its run from the front. its run from the front. its run from the front ( . b ( (b ) b Posture of ) ) Posture of Posture of the robot the the robot robot after completio after completion n of it of its s run fro run from m the side. the side. after completion of its run from the side. Figure 18. Robot end trajectory. Figure 18. Robot end trajectory. Figure 18. Robot end trajectory. Figure 19. Figure 19. The joint The joint trajectory after trajectory after the the in iver nver se ki se ki nemati nemati cs c s s s olo u lti uti on. on. In the fourth experiment, the Beckhoff controller, which is commonly used in the industry, was In the fourth experiment, the Beckhoff controller, which is commonly used in the industry, was used used and and the the general algor general algor ithm w ithm w as as implemented implemented in in th th e con e con troller. T troller. T hh e robo e robo t con t con trto rller oller sends m sends m otion otion commands to the drive at a certain period. This requires the controller to complete the kinematic commands to the drive at a certain period. This requires the controller to complete the kinematic inverse inverse sol sol uu titi on on with with in a s in a s pp ecif ecif ied amo ied amo uu nt nt of o time f time to to ensu ensu re re the co the co ntin ntin ui ui ty. ty. A A sh sh orter p orter p eriod eriod r r esu esu ltlt s s in a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sufficient in a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sufficient to m to m ee ete tthe the g g enera enera l l accur accur acy re acy re qu qu irem irem en en ts. ts. For For ex ex am am ple, in Re ple, in Re f. [5 f. [5 ] the ] the med med icia cla lrobot robot can can ac ac cept cept a a closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve a closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve a Appl. Sci. 2019, 9, x FOR PEER REVIEW 30 of 36 (a) (b) Figure 17. (a) Posture of the robot after completion of its run from the front. (b) Posture of the robot after completion of its run from the side. Appl. Sci. 2019, 9, 4365 30 of 36 Figure 18. Robot end trajectory. Figure 19. The joint trajectory after the inverse kinematics solution. Figure 19. The joint trajectory after the inverse kinematics solution. In the fourth experiment, the Beckho controller, which is commonly used in the industry, was In the fourth experiment, the Beckhoff controller, which is commonly used in the industry, was used and the general algorithm was implemented in the controller. The robot controller sends motion used and the general algorithm was implemented in the controller. The robot controller sends motion commands to the drive at a certain period. This requires the controller to complete the kinematic commands to the drive at a certain period. This requires the controller to complete the kinematic inverse solution within a speciﬁed amount of time to ensure the continuity. A shorter period results in inverse solution within a specified amount of time to ensure the continuity. A shorter period results a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sucient in a higher real-time performance. The period used in this article was 2 ms. The 2 ms cycle is sufficient to meet the general accuracy requirements. For example, in Ref. [5] the medical robot can accept a to meet the general accuracy requirements. For example, in Ref. [5] the medical robot can accept a closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve closed loop period of 20 ms. In other words, the proposed algorithm in this paper could achieve a a closed-loop period higher than industrial demand in equipment commonly used in the industry. The fast closed-loop period means that the algorithm has higher computational eciency. Since our algorithm could decompose complex inverse motion problems into several sub-problems and solve them one by one, all joint angles could be solved accurately and quickly. The ﬁnal experimental results show that the system could complete the inverse solution of the kinematics within the speciﬁed period. This allows the algorithm to run stably in situations with real-time requirements. Figure 8 shows the end curve planned to use 15 S-curve segments. The apparent rise and fall process in the jerk curve could be seen. This allows the entire curve to have a smoother transition at start–stop. Based on Equation (46) and the plan in Figure 10, the angular curve of each joint could be obtained, as shown in Figures 11–16. It can be seen from Figures 11–16 that all joint motion curves, as well as the various derivative curves could be ensured to start from 0 during the start-up phase smoothly. The continuous and smooth was kept in the whole moving process. Moreover, the curve was smoothly reduced to zero during the stop phase. Especially the fourth derivative had kept the continuous and smooth ﬂuctuations, which was the eect that could not be realized through traditional planning method. This paper introduced the joint curve obtained through using the traditional seven-segment planning there to facilitate the comparison, as shown in Figures 20–25. It can be clearly seen that the Jerk curve of each joint had an obvious jump phenomenon during the start–stop phase. Meanwhile, thumbing changes occurred in the operation process. These problems would cause the motor to operate unstably in the end. Appl. Sci. 2019, 9, x FOR PEER REVIEW 31 of 36 closed-loop period higher than industrial demand in equipment commonly used in the industry. The fast closed-loop period means that the algorithm has higher computational efficiency. Since our algorithm could decompose complex inverse motion problems into several sub- problems and solve them one by one, all joint angles could be solved accurately and quickly. The final experimental results show that the system could complete the inverse solution of the kinematics within the specified period. This allows the algorithm to run stably in situations with real-time requirements. Figure 8 shows the end curve planned to use 15 S-curve segments. The apparent rise and fall process in the jerk curve could be seen. This allows the entire curve to have a smoother transition at start–stop. Based on Equation (46) and the plan in Figure 10, the angular curve of each joint could be obtained, as shown in Figures 11–16. It can be seen from Figures 11–16 that all joint motion curves, as well as the various derivative curves could be ensured to start from 0 during the start-up phase smoothly. The continuous and smooth was kept in the whole moving process. Moreover, the curve was smoothly reduced to zero during the stop phase. Especially the fourth derivative had kept the continuous and smooth fluctuations, which was the effect that could not be realized through traditional planning method. This paper introduced the joint curve obtained through using the traditional seven-segment planning there to facilitate the comparison, as shown in Figure 20–25. It can be clearly seen that the Jerk curve of each joint had an obvious jump phenomenon during the start–stop phase. Meanwhile, thumbing changes occurred in the operation process. These problems would cause the motor to operate Appl. Sci. 2019, 9, 4365 31 of 36 unstably in the end. (a) (b) (c) (d) Figure 20. (a) Displacement of axis I over time by using seven segments S-curve. (b) Velocity of axis I Figure 20. (a) Displacement of axis I over time by using seven segments S-curve. (b) Velocity of axis Appl. Sci. 2019, 9, x FOR PEER REVIEW 32 of 36 over time by using seven segments S-curve. (c) Acceleration of axis I over time by using seven segments I over time by using seven segments S-curve. (c) Acceleration of axis I over time by using seven S-curve. (d) Jerk of axis I over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis I over time by using seven segments S-curve. (a) (b) (c) (d) Figure 21. (a) Displacement of axis II over time by using seven segments S-curve. (b) Velocity of axis Figure 21. (a) Displacement of axis II over time by using seven segments S-curve. (b) Velocity of axis II over time by using seven segments S-curve. (c) Acceleration of axis II over time by using seven II over time by using seven segments S-curve. (c) Acceleration of axis II over time by using seven segments S-curve. (d) Jerk of axis II over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis II over time by using seven segments S-curve. (a) (b) (c) (d) Figure 22. (a) Displacement of axis III over time by using seven segments S-curve. (b) Velocity of axis III over time by using seven segments S-curve. (c) Acceleration of axis III over time by using seven segments S-curve. (d) Jerk of axis III over time by using seven segments S-curve. Appl. Sci. 2019, 9, x FOR PEER REVIEW 32 of 36 (a) (b) (c) (d) Figure 21. (a) Displacement of axis II over time by using seven segments S-curve. (b) Velocity of axis II over time by using seven segments S-curve. (c) Acceleration of axis II over time by using seven segments S-curve. (d) Jerk of axis II over time by using seven segments S-curve. Appl. Sci. 2019, 9, 4365 32 of 36 (a) (b) (c) (d) Figure 22. (a) Displacement of axis III over time by using seven segments S-curve. (b) Velocity of axis Figure 22. (a) Displacement of axis III over time by using seven segments S-curve. (b) Velocity of axis Appl. Sci. 2019, 9, x FOR PEER REVIEW 33 of 36 III over time by using seven segments S-curve. (c) Acceleration of axis III over time by using seven III over time by using seven segments S-curve. (c) Acceleration of axis III over time by using seven segments S-curve. (d) Jerk of axis III over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis III over time by using seven segments S-curve. (a) (b) (c) (d) Figure 23. (a) Displacement of axis IV over time by using seven segments S-curve. (b) Velocity of axis Figure 23. (a) Displacement of axis IV over time by using seven segments S-curve. (b) Velocity of axis IV over time by using seven segments S-curve. (c) Acceleration of axis IV over time by using seven IV over time by using seven segments S-curve. (c) Acceleration of axis IV over time by using seven segments S-curve. (d) Jerk of axis IV over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis IV over time by using seven segments S-curve. (a) (b) (c) (d) Figure 24. (a) Displacement of axis V over time by using seven segments S-curve. (b) Velocity of axis V over time by using seven segments S-curve. (c) Acceleration of axis V over time by using seven segments S-curve. (d) Jerk of axis V over time by using seven segments S-curve. Appl. Sci. 2019, 9, x FOR PEER REVIEW 33 of 36 (a) (b) (c) (d) Figure 23. (a) Displacement of axis IV over time by using seven segments S-curve. (b) Velocity of axis IV over time by using seven segments S-curve. (c) Acceleration of axis IV over time by using seven segments S-curve. (d) Jerk of axis IV over time by using seven segments S-curve. Appl. Sci. 2019, 9, 4365 33 of 36 (a) (b) (c) (d) Figure 24. Figure 24. (a) Displacem (a) Displacement ent of of ax axis is V over ti V over time me by us by using ing seven s seven segments egments S-curve. S-curve. (b) ( V b elocity ) Velocity of axis of axis Appl. Sci. 2019, 9, x FOR PEER REVIEW 34 of 36 V over time by using seven segments S-curve. (c) Acceleration of axis V over time by using seven V over time by using seven segments S-curve. (c) Acceleration of axis V over time by using seven segments S-curve. (d) Jerk of axis V over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis V over time by using seven segments S-curve. (a) (b) (c) (d) Figure 25. (a) Displacement of axis VI over time by using seven segments S-curve. (b) Velocity of axis Figure 25. (a) Displacement of axis VI over time by using seven segments S-curve. (b) Velocity of axis VI over time by using seven segments S-curve. (c) Acceleration of axis VI over time by using seven VI over time by using seven segments S-curve. (c) Acceleration of axis VI over time by using seven segments S-curve. (d) Jerk of axis VI over time by using seven segments S-curve. segments S-curve. (d) Jerk of axis VI over time by using seven segments S-curve. Figure 18 shows the actual run in Cartesian space. It could be seen that the actual motion trajectory Figure 18 shows the actual run in Cartesian space. It could be seen that the actual motion coincided with the planned motion trajectory. The mean square error of the two tracks was calculated trajectory coincided wi 10 th the planned motion trajectory. The mean square error of the two tracks was to be 7.8265 10 m. This data shows that the error between the actual trajectory and the target −10 calculated to be 7.8265 × 10 m. This data shows that the error between the actual trajectory and the trajectory was extremely small. This shows that the inverse solution of the robot had higher accuracy. target trajectory was extremely small. This shows that the inverse solution of the robot had higher accuracy. 6. Conclusions Based on the DH model, this study proposed a universal algorithm for finding an inverse kinematics closed-form solution. This algorithm divided the inverse kinematics problem related to robots with a closed-form solution into three sub-problems assuming that the algebraic equation had a formula solution. The solvability conditions for the three types of sub-problems were analyzed to derive a formula solution. If a serial robot can be described with these three types of sub-problems, the robot will definitely have a closed-form solution. Meanwhile, the formula solution based on such a problem could quickly solve for all the joint angles. This method was not only applicable to six- DOF robots with a closed-form solution but also to low-DOF robots with the same solution form. In addition, establishing an algorithm based on the DH model provides a concise and efficient tool for selecting sub-problems and a real-time inverse kinematics solution for the motion controller of a serial robot. To verify the correctness and real-time performance of the algorithm, we presented four experimental designs. The first three experiments were conducted on MATLAB with a series of robot to verify the completeness, universality, and continuity of this algorithm. In the first experiment, the completeness of the algorithm was verified. The experiment used a low-degree-of-freedom robot with closed inverse solutions to prove that the algorithm could solve its closed inverse solution, and the algorithm would be terminated for the robot without closed inverse solutions to avoid infinite loop. This was used to prove the completeness of the algorithm. In the second experiment, two uncommon robots were constructed through using some basic problems, but the closed inverse solution could still be solved. Therefore, it could be known that the closed inverse solution of the series robots constructed by basic problems could be solved. Meanwhile, for these robots whose structure was not changed but the link parameter was changed, the closed inverse solution of them could also be solved correctly, which indicates that the algorithm had certain versatility in solving the closed inverse solution problem. In the third experiment, the common Puma560 robot was used as the experimental object. In the experiment, a spatial curve was planned. Appl. Sci. 2019, 9, 4365 34 of 36 6. Conclusions Based on the DH model, this study proposed a universal algorithm for ﬁnding an inverse kinematics closed-form solution. This algorithm divided the inverse kinematics problem related to robots with a closed-form solution into three sub-problems assuming that the algebraic equation had a formula solution. The solvability conditions for the three types of sub-problems were analyzed to derive a formula solution. If a serial robot can be described with these three types of sub-problems, the robot will deﬁnitely have a closed-form solution. Meanwhile, the formula solution based on such a problem could quickly solve for all the joint angles. This method was not only applicable to six-DOF robots with a closed-form solution but also to low-DOF robots with the same solution form. In addition, establishing an algorithm based on the DH model provides a concise and ecient tool for selecting sub-problems and a real-time inverse kinematics solution for the motion controller of a serial robot. To verify the correctness and real-time performance of the algorithm, we presented four experimental designs. The ﬁrst three experiments were conducted on MATLAB with a series of robot to verify the completeness, universality, and continuity of this algorithm. In the ﬁrst experiment, the completeness of the algorithm was veriﬁed. The experiment used a low-degree-of-freedom robot with closed inverse solutions to prove that the algorithm could solve its closed inverse solution, and the algorithm would be terminated for the robot without closed inverse solutions to avoid inﬁnite loop. This was used to prove the completeness of the algorithm. In the second experiment, two uncommon robots were constructed through using some basic problems, but the closed inverse solution could still be solved. Therefore, it could be known that the closed inverse solution of the series robots constructed by basic problems could be solved. Meanwhile, for these robots whose structure was not changed but the link parameter was changed, the closed inverse solution of them could also be solved correctly, which indicates that the algorithm had certain versatility in solving the closed inverse solution problem. In the third experiment, the common Puma560 robot was used as the experimental object. In the experiment, a spatial curve was planned. The correct joint angle sequence was obtained and the changes of the joint angle curve were continuously without jumping after been inversely solved by the algorithm. Which indicates that the algorithm could guarantee the continuity of its mapping on the inverse solution problem. The fourth experiment was conducted on a six-DOF robotic platform built using a Beckhoff-C6920 controller and an RGM motor to verify the real-time performance of the algorithm using the above hardware platform. The spatial and joint positions were computed with a close-loop cycle of 2 ms and were transmitted to the motor via CanOpen. The results demonstrated the ability of the algorithm to solve the three sub-problems within a specified cycle and to compute the appropriate joint angles. Moreover, the curve was continuous. This paper used a completely new approach to study the inverse kinematics of serial robots. In this study, the conditions for the existence of the closed-form inverse kinematic solution of a serial robot were completed and the basic theory of robotics was perfected. Based on this method, a general closed-form inverse kinematics algorithm based on the DH model was implemented, which had not been done in previous studies. Importantly, this method is a general-purpose algorithm that can be used in industrial fields in real time. This increases the versatility of the universal serial robot motion controller. However, the algorithm still has shortcomings. First, the algorithm only targets serial robots and the moving joints are not included in the solution model. This limits the scope of application of the algorithm, due to its inability to solve the cases of SCARA robots or other parallel robots. In addition, the algorithm cannot solve the problem when the robot passes through a singular point; singular points are avoided via restrictions implemented in the software. Moreover, because the algorithm is a purely analytical method, it is dicult to combine with other numerical methods to solve the problem of singular points. Author Contributions: Conceptualization, W.S.; methodology, W.S. and L.X.; software, W.S.; writing—original draft preparation, W.S.; writing—review and editing, W.S., L.X., H.B., L.Q.; supervision, L.Q., funding acquisition, L.X. Appl. Sci. 2019, 9, 4365 35 of 36 Funding: Supported by: National Key R&D Program of China (2016YFC0803000, 2016YFC0803005). Conﬂicts of Interest: The authors declare there is no conﬂict of interest regarding the publication of this paper. References 1. Xiao, W.; Strauß, H.; Loohß, T.; Homeister, H.W.; Hesselbach, J. Closed-form inverse kinematics of 6R milling robot with singularity avoidance. Prod. Eng. 2011, 5, 103–110. [CrossRef] 2. Wang, L.; Fallavollita, P.; Zou, R.; Chen, X.; Weidert, S.; Navab, N. Closed-form inverse kinematics for interventional C-arm X-ray imaging with six degrees of freedom: Modeling and application. IEEE Trans. Med. Imaging 2012, 31, 1086–1099. [CrossRef] [PubMed] 3. Khan, A.; Cheng, X.; Zhang, X.; Quan, W.L. Closed form inverse kinematics solution for 6-DOF underwater manipulator. In Proceedings of the 2015 International Conference on Fluid Power and Mechatronics (FPM), Harbin, China, 5–7 August 2015; pp. 1171–1176. 4. Bunathuek, N.; Laksanacharoen, P. Inverse kinematics analysis of the three-legged reconﬁgurable spherical robot II. In Proceedings of the 2017 3rd International Conference on Control, Automation and Robotics (ICCAR), Nagoya, Japan, 22–24 April 2017; pp. 31–35. 5. Bai, L; Yang, J.; Chen, X.; Jiang, P.; Liu, F.; Zheng, F.; Sun, Y. Solving the Time-Varying Inverse Kinematics Problem for the Da Vinci Surgical Robot. Appl. Sci. 2019, 9, 546. [CrossRef] 6. Hartenberg, R.S. A Kinematic Notation for Lower-Pair Mechanism Based on Matrices. Trans. ASME J. Appl. Mech. 1955, 22, 215–221. 7. Raghaven, M.; Roth, B. Kinematic analysis of the 6R manipulator of general geometry. In Proceedings of the International Symposium on Robotics Research, Hanoi, Vietnam, 6–10 October 2019; pp. 263–269. 8. Penrose, R. On Best Approximate Solutions of Linear Matrix Equations. Proc. Camb. Philos. Soc. 1956, 52, 17. [CrossRef] 9. Siciliano, B. A Closed-loop Inverse Kinematic Scheme for On-line Joint-based Robot Control. Robotica 1990, 8, 231–243. [CrossRef] 10. Wampler, C.W.I. Manipulator Inverse Kinematic Solutions Based on Vector Formulations and Damped Least-Squares Methods. IEEE Trans.Syst. Man Cybern. 1986, 16, 93–101. [CrossRef] 11. Kelemen, M.; Virgala, I.; Lipták, T.; Miková, L’.; Filakovský, F.; Bulej, V. A Novel Approach for a Inverse Kinematics Solution of a Redundant Manipulator. Appl. Sci. 2018, 8, 2229. [CrossRef] 12. Reiter, A.; Muller, A.; Gattringer, H. On Higher Order Inverse Kinematics Methods in Time-Optimal Trajectory Planning for Kinematically Redundant Manipulators. IEEE Trans. Ind. Inform. 2018, 14, 1681–1690. [CrossRef] 13. Feng, Y.; Wang, Y.; Wei, S. A novel hybrid electromagnetism-like algorithm for solving the inverse kinematics of robot. Ind. Robot 2011, 38, 429–440. [CrossRef] 14. Yin, F.; Wang, Y.N.; Wei, S.N. Inverse Kinematic Solution for Robot Manipulator Based on Electromagnetism-like and Modified DFP Algorithms. Acta Autom. Sin. 2011, 37, 74–82. [CrossRef] 15. Paul, R.P.; Shimano, B. Kinematic Control Equations for Simple Manipulators. In Proceedings of the IEEE Conference on Decision and Control Including the 17th Symposium on Adaptive Processes, San Diego, CA, USA, 10–12 January 1979. 16. Pieper, D.L. The Kinematics of Manipulators under Computer Control. Ph.D. Thesis, Stanford University, Stanford, CA, USA, 1968. 17. John, J.C. Inverse kinematics of the manipulator. In Introduction to Robotics: Mechanics and Control; Pearson: London, UK, 2005; pp. 87–88. 18. Siciliano, B.; Sciavicco, L.; Villani, L.; Oriolo, G. Robotics: Modelling, Planning and Control, 2nd ed.; Springer Publishing Company, Incorporated: London, UK, 2010; pp. 76–82. 19. Cui, H.-X.; Feng, K.; Li, H.-L.; Han, J.-H. Singularity avoidance of 6R decoupled manipulator using improved Gaussian distribution damped reciprocal algorithm. Ind. Robot 2017, 44, 324–332. [CrossRef] 20. Murray, R.M.; Sastry, S.S.; Li, Z. A Mathematical Introduction to Robotic Manipulation; CRC Press, Inc.: Florida, FL, USA, 1994; p. 292. 21. Kahan, W. Lectures on Computational Aspects of Geometry; University of California: Berkeley, CA, USA, 1983. 22. Paden, B. Kinematics and Control Robot Manipulators. Ph.D. Thesis, Department of Electrical Engineering and Computer Sciences, University of California, Berkeley, CA, USA, 1986. Available online: https://10.1109/ACSSC. 1985.671441 (accessed on 8 October 2018). [CrossRef] Appl. Sci. 2019, 9, 4365 36 of 36 23. Wang, H.; Lu, X.; Cui, W.; Zhang, Z.; Li, Y.; Sheng, C. General inverse solution of six-degrees-of freedom serial robots based on the product of exponentials model. Assem. Autom. 2018, 38, 361–367. [CrossRef] 24. An, H.S.; Seo, T.W.; Lee, J.W. Generalized solution for a sub-problem of inverse kinematics based on product of exponential formula. J. Mech. Sci. Technol. 2018, 32, 2299–2307. [CrossRef] 25. Corke, P.I.J.I.R.; Magazine, A. A robotics toolbox for MATLAB. IEEE Robot. Autom. Mag. 1996, 3, 24–32. [CrossRef] 26. Jazar, R.N. Inverse kinematics. In Theory of Applied Robotics; Springer: Berkeley, CA, USA, 2010; pp. 222–234. © 2019 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Journal

Applied Sciences – Multidisciplinary Digital Publishing Institute

Published: Oct 16, 2019

Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots

Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots

Get 20M+ Full-Text Papers For Less Than $1.50/day. Start a 14-Day Trial for You or Your Team.

Learn More →

Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots

Existence Conditions and General Solutions of Closed-form Inverse Kinematics for Revolute Serial Robots

References (31)

Abstract

Journal

Recommended Articles

There are no references for this article.

Our policy towards the use of cookies