A Closed-Form Solution for the Inverse Kinematics of the 2N-DOF Hyper-Redundant Manipulator Based on General Spherical Joint

applied sciences

Article A Closed-Form Solution for the Inverse Kinematics of the 2n-DOF Hyper-Redundant Manipulator Based on General Spherical Joint

Ya’nan Lou , Pengkun Quan, Haoyu Lin, Dongbo Wei and Shichun Di *

School of Mechatronics Engineering, Harbin Institute of Technology, Harbin 150001, China; [email protected] (Y.L.); [email protected] (P.Q.); [email protected] (H.L.); [email protected] (D.W.) * Correspondence: [email protected]; Tel.: +86-1390-4605-946

Abstract: This paper presents a closed-form inverse kinematics solution for the 2n-degree of freedom (DOF) hyper-redundant serial manipulator with n identical universal joints (UJs). The proposed algorithm is based on a novel concept named as general spherical joint (GSJ). In this work, these universal joints are modeled as general spherical joints through introducing a virtual revolution between two adjacent universal joints. This virtual revolution acts as the third revolute DOF of the general spherical joint. Remarkably, the proposed general spherical joint can also realize the decoupling of position and orientation just as the spherical wrist. Further, based on this, the universal joint angles can be solved if all of the positions of the general spherical joints are known. The position of a general spherical joint can be determined by using three distances between this unknown general spherical joint and another three known ones. Finally, a closed-form solution for the whole

manipulator is solved by applying the inverse kinematics of single general spherical joint section using these positions. Simulations are developed to verify the validity of the proposed closed-form inverse kinematics model. Citation: Lou, Y.; Quan, P.; Lin, H.; Wei, D.; Di, S. A Closed-Form Solution for the Inverse Kinematics of Keywords: closed-form inverse kinematics; general spherical joint; distance-based; hyper-redundant the 2n-DOF Hyper-Redundant manipulator Manipulator Based on General Spherical Joint. Appl. Sci. 2021, 11, 1277. https://doi.org/10.3390/ app11031277 1. Introduction To increase the manipulator’s kinematic dexterity, more degrees of freedom (DOFs) Received: 5 January 2021 beyond required for accomplishing a given task are introduced. These redundant DOFs can Accepted: 27 January 2021 be used to accomplish additional various tasks, including obstacle avoidance, singularity Published: 30 January 2021 avoidance, and physical limitations [1]. However, the inverse kinematics of the redundant manipulators would be more challenging and complicated than the non-redundant one Publisher’s Note: MDPI stays neutral because of the existence of infinite number of solutions for a given task [2]. To solve the with regard to jurisdictional claims in inverse kinematics of such redundant manipulators, many algorithms have been proposed. published maps and institutional affil- According to the solution type of the inverse kinematics problem, these algorithms can iations. be categorized into three groups: analytical method, numerical method, and the hybrid method [2–5]. Upon the analytical method, the joint variables can be explicitly interpreted by a function of the given pose of end-effector. Hence, the analytical method is characterized Copyright: © 2021 by the authors. by computational efficiency and can provide all solutions for a given pose within the Licensee MDPI, Basel, Switzerland. workspace of the manipulator [5]. However, the closed-form solution is strongly depen- This article is an open access article dent on the configuration of the manipulator. It is generally known that the common distributed under the terms and 6-DOF non-redundant manipulator has a closed-form inverse kinematics solution if three conditions of the Creative Commons consecutive revolute joint axes of the manipulator intersect at a common point or three Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ consecutive revolute joint axes of the manipulator are parallel [6], especially the former. 4.0/). Actually, for the redundant manipulators, adding reasonable constraints is an effective way

Appl. Sci. 2021, 11, 1277. https://doi.org/10.3390/app11031277 https://www.mdpi.com/journal/applsci Appl. Sci. 2021, 11, 1277 2 of 19

to pursuit the closed-form inverse kinematics solution. That is because that the number of DOF is bigger than the given conditions provided by the given position and orientation. Meanwhile, the existence of the above two criterions for the non-redundant 6-DOF manipulator will greatly increase the possibility of getting the closed-form solution [7–10]. For example, Srinivas N. et al. [7] modeled the redundant manipulator as a multi-section robot composed of spherical joints and proposed an approach, which computed per-section endpoint locations as the constraints. Based on these endpoint locations, the closed-form inverse kinematics of the multi-section robot is obtained by applying the inverse kinematics of single-section robot. Kai X. et al. [9] directly enumerated the different combinations of the segment positions as the constraints. Wenfu X. et al. [10] modeled the manipulator as a multi-group manipulator and determined the Cartesian coordinates of each node by modifying the well-known backbone curve method [11]. How to add right constraints to obtain the closed-form solution requires the algebraic and geometric intuition. In contrast to the analytical method, the solution of the joint variables solved by the numerical method is implicit in the nonlinear equations relating the joint variables and the given pose. Thus, various algorithms solving nonlinear equations can be applied to solve the inverse kinematics of the redundant manipulators, for example, Newton–Raphson algorithm [12], Levenberg–Marquardt algorithm [13], and Cyclic Coordinate Descent (CCD) method [14]. Moreover, intelligent algorithms, such as neural network [15,16], fuzzy logic [17], and generic algorithm [18] are also used for the inverse kinematics problem of the redundant manipulators. These algorithms inevitably bear sensibility to the choice of initial values, high computational burden, and numerical errors. Upon the hybrid method combining analytical and numerical method, part of the joint variables can be given by the closed-form algorithm and the rest are solved by the numerical method. Alternatively, the solution may be in closed-form, while the closed-form solution is based on the conditions provided by some numerical method like numerical optimization. For example, to solve the inverse kinematics of the proposed (2n + 1)-DOF hyper-redundant manipulator, Ananthanarayanan H. et al. [19] provided the first two and last three joint angles given by analytical expression, and the other joint angles solved by considering the optimization of elbow locations. Kim J.S. et al. [20] determined the position of elbow of the 7-DOF manipulator by minimizing the force imposed on the elbow and the joint angles are obtained in analytical expressions by using geometric approach. Kircanski M.V. et al. [21] proposed a method for a 7-DOF manipulator where four joint angles are computed analytically and the remaining three are computed using the pseudoinverse of Jacobian matrix. Due to the addition of the closed-form solution for some joint variables, the hybrid method can efficiently improve the computational efficiency [13,21]. In this work, the hyper-redundant robot is a kind of 2n-DOF serial manipulator with n identical universal joints. These universal joints have the same mechanical mechanism, the same angle limits, and the same kinematic characteristics. Research about the inverse kinematics problem for this type of manipulator has been carried out. However, these methods involve, more or less, numerical approaches. Huang H. et al. [22] studied the inverse kinematics of the manipulators with three universal joints and one rotary joint. The last universal joint and the rotary joint are regarded as a spherical wrist. Based on this, the closed-form solution of the last six joint angles is given by functions about the first joint angle. The first joint angle was determined by minimizing total energy. Therefore, this inverse kinematics solution is not a rigorous closed-form solution. Wenfu X. et al. [10] computed the Cartesian position of each universal joint by modifying the backbone curve method in which a spatial backbone is defined using a mode function to represent the geometry of the manipulator [11]. All of the universal joint angles are solved analytically based on the above computed universal joint positions. However, the numerical optimization still exists during the process of determining the Cartesian positions of the universal joints. Thus, this approach is not a rigorous closed-form solution. Tang, L. et al. [23] provided the inverse Appl. Sci. 2021, 11, 1277 3 of 19

kinematics of the single-section manipulator. However, for the inverse kinematics of the whole manipulator, the proposed approach fell into the Jacobian analysis. Compared to the above inverse kinematics solutions for the 2n-DOF hyper-redundant serial robot with n identical universal joints, the inverse kinematics solution proposed in this paper is rigorous closed-form. In this inverse kinematics model, the universal joint is modeled as a general spherical joint through introducing a virtual revolution between two adjacent universal joints. It is surprising that all of the joints of the 2n-DOF manipulator are general spherical joints. Simultaneously, these general spherical joints can realize the decoupling of the positions and orientations. A novel method for determining the positions of these universal joints is proposed. In this method, the Cartesian coordinates of a universal joint are obtained according to three distances between the universal joint to be solved and another three different known ones. According to inverse kinematics of a single general spherical joint section, the closed-form solution for the whole manipulator is solved. How to choose the distances for solving the positions of universal joints is important in the proposed closed-form solution. The main contribution of this paper is as follows. Firstly, a novel rule for link frames attached to the links is proposed during the process of forward kinematics, which is more intuitive and briefer than the classical (Denavit–Hartenberg) D–H convention. Secondly, a novel concept named “general spherical joint”, which exists between any two adjacent link frames, is introduced. The general spherical joint is similar to spherical wrist and can realize the decoupling of the positions and orientations. Thirdly, a completely closed-form solution for the 2n-DOF manipulator is proposed. This solution is not involved with any numerical method. Moreover, the proposed method to obtain the positions of universal joints is extremely geometrically intuitive. This directly contributes to the ease to understand of the proposed closed-form solution compare to the above inverse kinematics solutions. The remaining of the paper is organized as follows. Section2 sets a novel rule for link frames and accomplishes the forward kinematics of the proposed 2n-DOF hyper-redundant serial robot. Section3 introduces the deﬁnition of “general spherical joint”, and solves the inverse kinematics problem by introducing the certain distances. Section4 veriﬁes the validity of the proposed kinematics model. Finally, Section5 concludes this paper.

2. Forward Kinematics The forward kinematics of the 2n-DOF hyper-redundant serial robot is described in this section. The studied manipulator is a serial robot consisting of n identical universal joints (UJ). For displaying purpose, the value of n is set to five. Figure1a,b shows the link frame assignment for this manipulator, where the link frame refers to the coordinate system fixed on the link according to certain rule. Figure1d gives the actual mechanical mechanism corresponding to Figure1b. For the purpose of intuitiveness and convenience during kinematics analysis, the rule for link frames no longer follows the Denavit–Hartenberg (D–H) convention, and a novel rule is adopted to define Frame i for Link i:

• Locate the origin Oi of Frame i at the intersection of the two axes of UJ i + 1; • Choose axis zi along the central axis of Link i; • Choose axis yi along the common axis (Axis 1 of UJ i + 1) of the two mounting holes (mounting holes 1 and 2) of Link i for UJ i + 1; • Choose axis xi to complete a right-handed frame. At this time, this axis coincides with the Axis 2 of UJ i + 1 when the corresponding rotation angle γi+1 of UJ i + 1 is equal to zero. When the angle γi+1 is not equal to zero, the coincidence no longer exists.

When UJ joint angles are all equal to zero, a constant angle αi exists between link Frame i and link Frame i − 1 about the central axis of Link i, as shown in Figure1c. Figure1e gives the actual mechanical mechanism corresponding to Figure1c. Appl. Sci. 2021, 11, x FOR PEER REVIEW 4 of 19 Appl. Sci. 2021, 11, 1277 4 of 19

zi A zi Mounting hole 1 Mounting hole 1 z5 Axis 2 of UJ i+1 Axis 1 of UJ i+1 Connecting to UJ i+1 End-effector UJ i+1 Mounting holes O5 y5 of Link i+1 Oi Oi

x5 z4 Link 5 5

l Mounting hole 2 i+1 γi+1 yi γ yi O4 xi Mounting hole 2 UJ 5 xi βi+1 y4 βi+1 Link i γ5 z3 β5 Link i

4 Link 4

l x4

UJ 4 O3 y3 γ4 z2 β4 (b) (d) 3 Link 3 l x3 A UJ 3 O2 y5 A y5 y2 y4 y4 γ3 z1 α4 y3 α4 y3 2 3 l x2 β Link 2 y2 y2 UJ 2 O1 α2 y1 α2y1 y0 y0 2 y1 O0,...5

γ z0 z0, ,5 1 l x1 β2 Link 1

UJ 1 0 O z0, ,5 γ1 y0 x0 α5 x5 α5 x5 β1 Link 0 (Base) α3 α1 x4 3 x4 x3 O0,...5 α x2 α1 x3 0 x1 x2 x x0 x1 (a) (c) (e) Figure 1. TheFigure studied 1. The2n-degree studied of 2 freedom푛-degree (DOF) of freedom redundant (DOF manipulator) redundant (n = manipulator 5): (a) layout of(n the = 5 link): (a frames,) layout universal of joints, andthe links; link (b )frames, a novel ruleuniversal for deﬁning joints, link and Frame links;i; ( (c)b) thea novel constant rule angle for defining between linklink Frame Framei and 푖; (c link) the Frame con-i − 1; (d) the actualstant mechanical angle between mechanism link correspondingFrame 푖 and tolink (b );Frame (e) the 푖 actual− 1; ( mechanicald) the actual mechanism mechanical corresponding mechanism to cor- (c). responding to (b); (e) the actual mechanical mechanism corresponding to (c). Once the link frames have been established, the position and orientation of Frame i Once the withlink frames respect tohave Frame beeni − established,1 can be completely the position determined and orientation by the kinematic of Frame parameters 푖 including link length li, UJ joint angles βi, γi, and the constant angle αi. Based on these kine- with respect to Frame 푖 − 1 can be completely determined by the kinematici−1 parameters matic parameters, the homogeneous transformation matrix Ai expressing the position including link length 푙푖, UJ joint angles 훽푖, 훾푖, and the constant angle 훼푖. Based on these and orientation of Frame i with respect to Frame i − 1 can푖 be−1 given by kinematic parameters, the homogeneous transformation matrix 푨푖 expressing the position and orientation of Frame 푖 withi−1 respect= R to( Frame)R ( 푖 −)R1 can( )beT given( ) by Ai yi−1 βi xi−1 γi zi−1 αi zi−1 li (1)

R 푖−1 R R T where yi−1 (βi), xi−1 (γi(), )zi−1 (αi(), and) zi−(1 (l)i) are four( ) homogeneous transformation 푨푖 = 퓡푦푖−1 훽푖 퓡푥푖−1 훾푖 퓡푧푖−1 훼푖 퓣푧푖−1 푙푖 (1) matrices describing the basic rotations by angle βi about axis yi−1, by angle γi about axis x , by angle α about axis z , and the basic translation by l along axis z , respectively. where 퓡 (훽i)−,1 퓡 (훾 ),i 퓡 (훼 ),i − and1 퓣 (푙 ) are four homogeneousi transfor-i−1 푦푖−1 푖 Substituting푥푖−1 푖 the푧푖− detailed1 푖 form of푧푖 the−1 four푖 basic transformations in Equation (1) gives mation matrices describing the basic rotations by angle 훽푖 about axis 푦푖−1, by angle 훾푖  + − +  about axis 푥푖−1, by angle 훼푖 about axiscαi cβ i푧푖−s1α,i sandγi sβ i thesα ibasiccβi ctranslationαi sγi sβi cγi sβ ibylic γ푙i푖s βalongi axis 푧 , respectively. i−1  sα sγ cα cγ −sγ − lisγ  푖−1 A =  i i i i i i  (2) i −c s + s s c −s s + c s c c c l c c Substituting the detailed form of theαi β fouri α basici γi βi transformationsαi βi αi γi βi inγ iEquationβi i γi βi (1) gives 0 0 0 1

푐훼 푐훽 + 푠훼 푠훾 푠훽 −푠훼 푐훽 + 푐훼 푠훾 푠훽 푐훾푖푠훽푖 푙푖푐훾푖푠훽푖 where cα , sα푖 , c푖β , sβ 푖, cγ푖 , and푖 sγ represent푖 푖 푖 cos푖 α푖i, sin αi, cos βi, sin βi, cos γi, and sin γi, i i i i i i −푠 −푙 respectively.푖−1 For푠 one훼 푐훾 actual manipulator푐훼 푐 studied훾 in this paper,훾푖 li푖and푠훾 αi are constant, thus, 푨푖 = 푖 푖 푖 푖 푖 , (2) the transformation−푐 푠 + matrix푠 푠 푐 from Frame푠 푠 to+ Frame푐 푠 푐i − 1 given푐 푐 by (1) is function of the two UJ 훼푖 훽푖 훼푖 훾푖 훽푖 훼푖 훽푖 훼푖 훾푖 훽푖 훾푖 훽푖 푙푖푐훾푖푐훽푖 joint angles,[ i.e., βi0and γi. 0 0 1 ] Based on Equation (1), the forward kinematics, i.e., the position and orientation of Frame n with respect to Frame 0, can be given by

where 푐훼푖 , 푠훼푖 , 푐훽푖 , 푠훽푖 , 푐훾푖 , and 푠훾푖 represent0 cos0훼푖1, sin 훼n푖−,1 cos 훽푖 , sin 훽푖 , cos 훾푖 , and Tn = A1A2 ··· An (3) sin 훾푖, respectively. For one actual manipulator studied in this paper, 푙푖 and 훼푖 are constant, thus, the transformation matrix from Frame 푖 to Frame 푖 − 1 given by (1) is function of the two UJ joint angles, i.e., 훽푖 and 훾푖. Based on Equation (1), the forward kinematics, i.e., the position and orientation of Frame 푛 with respect to Frame 0, can be given by Appl. Sci. 2021, 11, x FOR PEER REVIEW 5 of 19

0 0 1 푛−1 푻푛 = 푨1푨2 ⋯ 푨푛 (3)

Appl. Sci. 2021, 11, 1277 5 of 19 For unified symbol, the following notation will be used: 푖−1  푹푖 and 푹푖 denote the rotation matrix of the homogeneous transformation matrix 푖−1 0 푨For푖 uniﬁed and 푻 symbol,푖 , respectively. the following notation will be used:  푖−i−11 • 풑R푖i andandR 풑i denote푖 denote the rotationthe position matrix vector of the homogeneous of the homogeneous transformation transformation matrix matrix 푖i−−11 0 0 푨A푖i andandT i푻, respectively.푖 , respectively. 푖−1 푖−1 푖−1  ( i−1 ) (풏푖 풔푖 풂푖) • p풏i 푖 and풔푖pi denote풂푖 the and position vector of the denote homogeneous the expressions transformation of the matrix rotation matrix 푖i−−11 0 푹A푖i andandT i 푹, respectively.푖, respectively. 푇 푇 푇 푇 • 푖−i−1 1 푖i−1 i−푖−1 1 푖−1 푖−1 푖−1 푖−1 푖−1 푖−1 푖−1 푖−1 푖−1  (푛푖n,푥i 푛s푖i,푦 a푛i 푖,푧 )and, (푠푖n,푥i si푠푖,푦ai denote푠푖,푧 ) the, ( expressions푎푖,푥 푎푖,푦 of the푎푖, rotation푧 ) , (푝 matrix푖,푥 푝푖,푦 푝푖,푧 ) i−1 푇 푇 푇 푇 andRi and(푛푖,푥Ri, respectively.푛푖,푦 푛푖,푧) , (푠푖,푥 푠푖,푦 푠푖,푧) , (푎푖,푥 푎푖,푦 푎푖,푧) , (푝푖,푥 푝푖,푦 푝푖,푧) de- T T T i−1 i−1 i−1 i−푖1−1 i−푖1−1 i−1푖−1 푖−i1−1 i−1 i−1 • noteni, xthe nexpressionsi,y ni,z , of s풏i,x푖 ,s i,풔y푖 s, i,z풂푖 ,, 풑푖ai,x andai,y 풏푖,a i,풔z 푖, 풂푖,, 풑푖, respectively. T T T pi−1 pi−1 pi−1 and n n n , s s s , 3. Inversei,x Kinematicsi,y i,z i,x i,y i,z i,x i,y i,z T T i−1 i−1 i−1 ai,x ai,y ai,z , pi,x pi,y pi,z denote the expressions of ni , si , ai , 3.1. Generali−1 Spherical Joint pi and ni, si, ai, pi, respectively. To solve the inverse kinematics and illustrate the essence of the kinematics for the 2푛3.-DOF Inverse redundant Kinematics manipulator with 푛 identical UJs, a novel concept named as “general 3.1. General Spherical Joint spherical joint” (GSJ) is introduced. GeneralTo solve thespherical inverse joint kinematics is inspired and illustrate by the the homogeneous essence of the kinematicstransformation for the between any 2n-DOF redundant manipulator with n identical UJs, a novel concept named as “general twospherical adjacent joint” frames. (GSJ) is introduced.Notice that the rotation component of the homogeneous transfor- mationGeneral matrix spherical is obtained joint is inspiredby multiplying by the homogeneous three basic transformation rotation matrices between any about three or- thonormaltwo adjacent axes. frames. Based Notice on that this the fact, rotation the componentjoint between of the any homogeneous two adjacent transforma- frames can be re- gardtion matrixas a spherical is obtained joint, by multiplying i.e., the threenew basic concept rotation general matrices spherical about three joint. orthonormal In accordance with axes. Based on this fact, the joint between any two adjacent frames can be regard as a thespherical spherical joint, wrist, i.e., the GSJ new also concept possesses general spherical the property joint. In of accordance three consecutive with the spherical revolute joint axes intersectingwrist, GSJ also at possesses a common the propertypoint. These of three three consecutive revolute revolute joint joint axes axes include intersecting the two axes of 푖- that UJ a commonand the point. central These axis three of Link revolute 푖, as joint shown axes include in Figure the two 2. The axes ofrevolutioni-th UJ and about the the central central axis of Link i, as shown in Figure2. The revolution about the central axis of Link axis of Link 푖 is embodied in the constant angle 훼푖 between link Frame 푖 and link Frame i is embodied in the constant angle α between link Frame i and link Frame i − 1. Thus, 푖 − 1. Thus, this revolution is virtualizedi as the third DOF of the GSJ to form the three this revolution is virtualized as the third DOF of the GSJ to form the three revolute axes revoluteintersecting axes at aintersecting common point. at a common point.

αi Revolute axis 3 (Virtual revolution)

yi xi

Link i

UJ i

Oi-1 xi-1 yi-1

γi Link i-1

Revolute axis 2 βi Revolute axis 1 FigureFigure 2. 2. General spherical spherical joint. joint.

Based on the GSJ, the studied 2푛-DOF manipulator can be regard as a serial robot, whose links are connected by 푛 identical GSJs. Simultaneously, the GSJ can also realize Appl. Sci. 2021, 11, 1277 6 of 19

Based on the GSJ, the studied 2n-DOF manipulator can be regard as a serial robot, whose links are connected by n identical GSJs. Simultaneously, the GSJ can also realize the decoupling between the position and orientation just as the spherical joint. Thus, considering the GSJ i − 1 in Figure2, the relationship between these two adjacent locations pi−1 and pi can be given by pi−1 = pi − liai (4) The manipulator studied in this paper is a strange robot. Most series robot commonly has only one spherical joint, which often acts as the last joint. This joint is called as spherical wrist, which can decouple the solutions for the position and for the orientation of end- effector. However, according to Equation (4), all joints of the studied manipulator can realize the decoupling between position of this joint and orientation of the corresponding end-effector. This fact is essential for the following inverse kinematics derivation of the whole manipulator.

3.2. Closed-Form Solution for Single-GSJ Section

The kinematic parameters αi, βi and γi for a single-GSJ section shown in Figure2 can i−1 be determined for the given orientation Ri of Frame i with respect to Frame i − 1. More- over, the solution of these kinematic parameters is in closed-form expression. Considering the actual mechanical structure of GSJ, the range of the three joint angles αi, βi and γi can be given by αi ∈ [−π/2, π/2], βi ∈ (−π/2, π/2) and γi ∈ (−π/2, π/2), respectively. According to (1), αi, βi and γi can be given by

 i−1 ni,y −  tan−1 , i f si 1 6= 0 si−1 i,y = i,y αi (5)  i−1 i−1  sgn ni,y ·π/2, i f si,y = 0

and ai−1 −1 i,x βi = tan i−1 ai,z −ai−1 , (6) −1 i,y γ = tan r i 2 2 i−1 + i−1 ni,y si,y

respectively, where cγi > 0 is exploited in Equation (5) and the ﬁrst equation of Equation (6), and sgn(·) denotes the signum function. For actual manipulator, the value of angle αi is constant. Hence, for any given desired i−1 orientation Ri within the workspace of the single-GSJ section, the computed value of αi by using Equation (5) should be equal to the actual value. From Equation (6), the value of i−1 βi and γi only depend on the component ai of the given rotation matrix. This is another important fact for the following inverse kinematics of the whole manipulator.

3.3. Closed-Form Solution for Multiple-GSJ Section This section illustrates the process of solving the inverse kinematics problem of the whole manipulator studied in this paper. The inverse kinematics derived in the previous section for a single-GSJ section can be iteratively applied to multiple serial GSJ sections i−1 to model an n-GSJ section manipulator. Given a list of ai (i = 1, ··· , n), the values of βi and γi can be computed for each GSJ section by applying the single-GSJ inverse i−1 kinematics. The list of ai (i = 1, ··· , n) can be determined by providing a list of locations pi (i = 0, ··· , n − 1) of GSJs i (i = 0, ··· , n − 1). This section proposes a distance-based algorithm to determine the list of locations. Therefore, the inverse kinematics problem for multiple-GSJ section can be transformed to the determination of the x, y and z coordinates pi of the GSJs i (i = 0, ··· , n − 1) and the i−1 determination of ai based on the above GSJ coordinates. Finally, the solution of the GSJ angles is got by using the inverse kinematics of single-GSJ section. The information ﬂow Appl. Sci. 2021, 11, x FOR PEER REVIEW 7 of 19

푖−1 0, ⋯ , 푛 − 1) and the determination of 풂푖 based on the above GSJ coordinates. Finally, the solution of the GSJ angles is got by using the inverse kinematics of single-GSJ section. Appl. Sci. 2021, 11, x FOR PEER REVIEW 7 of 19 The information flow during the process of solving the inverse kinematics of the whole manipulator is shown in Figure 3. Appl. Sci. 2021, 11, 1277 7 of 19

푖−1 Distance-based0 ,algorithm⋯ , 푛 − 1) and the determination(2), (3) of 풂푖 based(5) on the above GSJ coordinates. Finally, T 0 p ai1 , n the solution of thei GSJ angles is got byi using the inverse kinematicsii of single-GSJ section. Theduring information the process flow of during solving the the process inverse of kinematics solving the of theinverse whole kinematics manipulator of the is shownwhole Figure 3. Informationmanipulatorin Figure flow 3of. the is shownproposed in Figure closed -3.form solution.

3.3.1. Locations of the GSJs 0 Distance-based algorithm (2), (3) i1 (5) Tn pi 0 ai ii, Given the desired position and orientation 푻푛 of the end-effector, the location of GSJ 푶푛−1 can be computedFigureFigure 3. 3. usingInformationInformation Equation flow ﬂow of of( 4the the) and proposed proposed is given closed closed-form by-form 풑푛− solution1 solution.= 풑푛. − 푙푛풂푛. The location of GSJ 푶 is constant and is set as 풑 = [0 0 0]푇. To determine the locations of the 0 3.3.1. Locations of the GSJs푖−1 푶 3.3.1.푖 = 1 Locations, ⋯ , 푛 − 2 of the GSJs rest of GSJs 푖 ( ), a distance-based algorithm is0 proposed, as shown in Fig- Given the desired position and orientation T0n of the end-effector, the location of GSJ ure 4. This algorithmGiven introduces the desired two position distances and 푙orientation and 푙푻푛 basedof the endon -thiseffector, information, the location of GSJ O 푣,푖+1 푂푖 p = p − l a 푶 n−1 cancan be be computed computed using using Equation Equation ( (4)4) and and is is given given by by 풑n−1 = 풑 n − 푙 n풂n. The The location location where the 푥, 푦 and푛−1 푧 coordinates of a three-dimensional pointT can 푛 be−1 determined푛 푛 푛 by 푇 ofof GSJ GSJ O푶0 isis constantconstant andand isis set set as asp i풑−푖1−1== [0 0 00] . .To To determine determine the the locations locations of of th thee three distances between this unknown= ··· point− and another three known points. restrest of of GSJs GSJs 푶O푖 i ((푖i = 11,, ⋯ , 푛,−n 2),2 a), distance a distance-based-based algorithm algorithm is proposed, is proposed, as shown as shown in Fig- in As shown in Figure 4, 푙푣,푖+1represents the distance between the unknown 푶푖 and ureFigure 4. This4. This algorithm algorithm introduces introduces two two distances distances 푙 lv,i+ and1 and 푙 lO ibasedbased on on this this information, information, the known 푶 , and 푙 represents the distance between푣 ,푖+ the1 unknown푂푖 푶 and the 푖+where2where the푂 푖x푥,,y 푦and andz coordinates푧 coordinates of a of three-dimensional a three-dimensional point point can be can determined푖 be determined by three by known 푶0. threedistances distances between between this unknown this unknown point point and another and another three three known known points. points. As shown in Figure 4, 푙푣,푖+1represents the distance between the unknown 푶푖 and 푶 푙 End-effector 푶 the known 푖+2 , and 푂푖 represents theOn distance between the unknown 푖 and the

known 푶0. Link n ln

On-1 GSJ i+2 Link i+2 End-effector GSW (GSJ n) On Oi+1 li+2 GSJ i+3 Link n ln Link i+1 pi+1 pi+2 li+1 Oi+2 GSJ i+2 GSJ i+3 GSJ i+1 On-1 Oi+1 lv, i+1 GSJ i+2 Link i+2 Oi+2 GSW (GSJ n) li+1 Link i Oi+1 pi Oi li+2 v, i+1 Link i+1 GSJ i+3 l pi+1 li+1 GSJ i+1 pi+2 Oi+2 GSJ i+3 li GSJ i – the i-th general spherical joint Oi GSJ i+2 GSJ i+1 Oi+1 lv, i+1 Oi+2 Oi –the location of GSJ i li+1 1 Link i pi GSJ 2 O li – the lengthOi of Link i lv, i+1 GSJ i+1 lOi pi –l ithe position of theGSJ location i – the Oi-thi general sphericallOi joint Oi l1 Link 1 lOi - the distance betweenOi – Othei andlocation O0 of GSJ i GSJ 2 O1 lv,i+1 - the distance betweenli – the O lengthi and Oofi +Link2 i GSJ 1 lOi pi – the position of the location Oi lOi O0 l1 Link 1 lOi - the distance betweenp0 Oi and O0 Base lv,i+1 - the distanceGSJ between 1 Oi and Oi+2 O0 GSJ 1 O0 p0 Base Figure 4. Distance-based algorithm for determining the x, y, and z coordinatesGSJp 1 of the GSJs i (i = 0, ··· , n − 1). Figure 4. Distance-based algorithm for determining the 푥, 푦, and 푧 coordinatesi 0 풑 of the GSJs 푖 O 푖 (푖 = 0, ⋯ , 푛 − 1). As shown in Figure4, l represents the distance between the unknown O and the Figure 4. Distance-based algorithmv,i+1 for determining the 푥, 푦, and 푧 coordinates 풑푖 of thei GSJs 푖 (known푖 = 0, ⋯ O, 푛i+−21,). and lOi represents the distance between the unknown Oi and the known O0. Combining 푙푣,푖+1, 푙푂푖 with the third known distance 푙푖+1, i.e., link length, gives Combining lv,i+1, lOi with the third known distance li+1, i.e., link length, gives Combining 푙푣,푖+1, 푙푂 with the third known distance 푙푖+1, i.e., link length, gives ‖풑 푖− 풑 ‖2 = (푙 )22 2 푖 푖+1 kpi − p푖+i+11k = (li+1) 2kp‖풑− −p 풑 k22‖=2 =(l(푙 ))22 . (7) ‖풑푖 − 풑푖+2‖ =i (푖푙푣,i푖++2푖1+)1 . v,i+푖+11 (7) 2 2 2 ‖kp − p 2k‖2= lO ‖풑 − 풑 ‖2 =풑푖(i−푙 풑)푖0+2 = (푙푣i ,푖+1) . (7) 푖 0 푂푖 2 ‖풑 − 풑 ‖2 = (푙 ) In fact, Equation (7) provides the푖 basic0 position푂푖 information for the location of GSJ In fact, EquationOi according (7) provides to the relative the basic distances position of fourinformation locations offor GSJs, the location i.e., unknown of GSJOi , known O In, O fact,, andEquationO . Further (7) provides calculation the basic of Equation position (7)information gives a quadratic for the location equation of about GSJ 푶푖 according to thei+ 1relativei+2 distances0 of four locations of GSJs, i.e., unknown 푶푖, known 푶 according to the relative distances of four locations of GSJs, i.e., unknown 푶 , known 푶 , 푶 , and the푶푖 .unknown Further calculationpi,z: of Equation (7) gives a quadratic equation about푖 푖+1 푖+2 푶 0 , 푶 , and 푶 . Further calculation 2of Equation (7) gives a quadratic equation about 푖+1 푖+2 0 a∆(pi,z) + b∆pi,z + c = 0, (8) the unknown 푝푖,푧: the unknown 푝푖,푧: 2 where a = 1 + b2 + b2, b = −2b a − 2b a , c = a2 + a2 − l , 1 2 2 1 1 2 2 1 2 Oi − 2 − pi+1,y A1푎 ∙pi(+푝2,y A)2 + 푏 ∙ 푝 pi++2,z p푐i+=1,y 0pi+2,y pi+1,z pi+1,x A1−pi+2,(x8A)2 a = 푖,푧 , b 푎=∙ (푖,푝푧푖,푧) + 푏 ∙,푝 푖,푧 + 푐 =, a0, = − , (8) 1 pi+2,x pi+1,y−pi+2,y pi+1,x 1 pi+2,x pi+1,y−pi+2,y pi+1,x 2 pi+2,x pi+1,y−pi+2,y pi+1,x 2 2 2 2 2 2 p p −p p lO +(lO ) −2(lv,i+1) 2 lO +(lO ) −(li+1) 푎 = 1 + 푏2 + 푏b 2푎==푏−=1 +i−+2,푏2z2푏i+1,푎푏x 2−i+푏22,=x푏 i−+푎1,2z푏; 푐A푎=−=푎22푏+i푎+푎2 2 푐−=(푙푎i 2 )+ 푎2 − (,푙 A) = i+1 i . where where1 2 , pi+2,x1pi+11,y1−2 ,p i+2,y p2i+1,2x, 1 1 1 2 2,2 2푂1푖 , 2 푂푖 2 , 2 Appl. Sci. 2021, 11, 1277 8 of 19

Obviously, b1 and b2 are constant. a1 and a2 are quadratic functions about both

lv,i+1 and lOi . Hence, a is constant; b and c are functions about both lv,i+1 and lOi , = = i.e., b b lv,i+1, lOi , c c lv,i+1, lOi . a1, a2, b1 and b2 exist in the situation where pi+2,x pi+1,y − pi+2,y pi+1,x 6= 0 is satisﬁed. Further, the x, y and z coordinates of the location of GSJ Oi is given by √ −b± ∆ pi,z = 2a pi,x = a1 − b1 pi,z , (9) pi,y = a2 − b2 pi,z

= 2 − ≥ = where ∆ b 4ac 0, which is a function about lOi and lv,i+1, i.e., ∆ ∆ lv,i+1, lOi ; the 2 2 value of a is always nonzero because a is equal to 1 + b1 + b2. Given the desired position and orientation of end-effector and link lengths of the studied 2n-DOF serial manipulator, based on the known locations On−1, On and the

distances lv,n−1 and lOn−2 , this algorithm ﬁrstly determines the location of GSJ On−2, which = is function about lv,n−1 and lOn−2 , i.e., pn−2 pn−2 lv,n−1, lOn−2 . Then based on the

above computed On−2, known On−1 and the distances lv,n−2 and lOn−3 , this algorithm

determines the location of GSJ On−3, which is function about lv,n−2, lOn−3 , lv,n−1 and lOn−2 , = i.e., pn−3 pn−3 lv,n−2, lOn−3 , lv,n−1, lOn−2 . This process is repeated with the remaining

locations. Finally, based on the above computed O2, O3 and the distances lv,2 and lO1 , this

algorithm determines the location of GSJ O1, which is function about lv,2 and lO1 , ... , lv,n−2 = ··· and lOn−2 , i.e., p1 p1 lv,2, lO1 , , lv,n−1, lOn−2 , where lO1 is equal to link length l1. Hence, considering the whole manipulator, the positions of the GSJs p1,··· ,n−2 is

function about lv,2 and lO1 , ... , lv,n−2 and lOn−2 , i.e., = ··· p1,··· ,n−2 p1,··· ,n−2 lv,2, lO1 , , lv,n−1, lOn−2 . Based on this, the distances lv,2 and lO1 ,

..., lv,n−1 and lOn−2 should also satisfy the following inequality constraints:

∆(L ) = (b(L ))2 − 4a(L )c(L ) ≥ 0 i i i i , (10) pi+2,x(Li)pi+1,y(Li) − pi+2,y(Li)pi+1,x(Li) 6= 0

··· − = ··· where the value of i includes 1, , n 2, Li lv,i+1, lOi , , lv,n−1, lOn−2 represents the related variables for the corresponding i, and the first equation defines the workspace of the mechanism, while the second represents a singular configuration in which the two consecutive Links i + 1 and i + 2 are parallel. In particular, pi+2 = pn, pi+1 = pn−1 in the − = = situation where i is equal to n 2; pi+2 pn−1, pi+1 pn−2 lv,n−1, lOn−2 in the situation where i is equal to n − 3.

3.3.2. Determination of lv,i+1 and lOi

This sub-section discusses how to determine the distance lv,i+1 and lOi involved in distance-based algorithm. Considering the joint limits of GSJ angles, the value of the two distances is restricted by the following inequalities:

l l ≤ lv,i+1 ≤ li+1 + li+2 v,i+1 , (11) l ≤ ≤ i lO,i lOi ∑j=1 lj

l l where lv,i+1 and lO,i are the lower bound of lv,i+1 and lOi considering the joint limits, respec- l i l tively, i.e., lv,i+1 = minkpi+2(βi+1, γi+1, βi+2, γi+2)k, lO,i = minkpi(β1, γ1, ··· , βi, γi)k, i i pi+2 denotes the position vector of the homogeneous transformation matrix Ai+2 = i i+1 Ai+1(βi+1, γi+1)Ai+2(βi+2, γi+2), and these joint angles β1, γ1, ··· , βi+2, γi+2 have their own boundaries. In actual detection tasks, the hyper-redundant manipulator’s end effector needs to be maintained at a target point in a narrow space with a ﬁxed position and orientation, as shown in Figure5, and all of the manipulator’s links cannot collide with the environment. Appl. Sci. 2021, 11, 1277 9 of 19

Assume that the structural environment of the narrow space is known. The minimum distance between Link i (i = 1, 2, 3, 4) and the environmental boundary can be expressed as

di,min = min kAi(u) − Ci,vk, (12) u∈[0,1],v

A ( ) O O Appl. Sci. 2021, 11, x FOR PEER REVIEW where i u represents the point on the straight line i−1 i where10 of 19 Link i is, and its coordinate value is

p (u) = p l , l + up − p l , l . (13) Ai i−1 v,i+1 Oi i i−1 v,i+1 Oi

C4,v Fixed position and d4,min O4 orientation O3 l4 A4(u) Target point lv,4 lv,3 lO,3 l3 Environmental 3,v boundary lv,2 C d3,min O0 Narrow C2,v A3(u) lO,2 lv,1 space O2 d1,min l2 C1,v l1 lO,1 d2,min O1 A1(u) A2(u)

Figure 5. Collision detection for a ﬁve-general spherical joint (GSJ) manipulator. Figure 5. Collision detection for a five-general spherical joint (GSJ) manipulator. The symbol u ∈ < represents the linear parameter, when u ∈ [0, 1], A (u) only repre- By combining the boundary distribution, and taking the avoidance of collision be- i sents the point on the link’s axis, and Ci v represents the point on the narrow environment tween the links and the boundary as the criterion, the distances, 푙푣,푖+1 and 푙푂 can be used i 푖 l l u to describe boundarythe manipulator’s that is overall closest configuration to Link . In in actual a narrow calculation, space scene the more distances intui- v,i+1, Oi and in tively. This Equationis the reason (13) why are the given two distances by are chosen as the parameters of the inverse kinematics solution process. Take Figure 6 as an example, the environment has four = l + · = ··· boundaries, and the coordinate systemlv,i+ 1originslv,i+ 푶1 푖+1,k푶1푖+∆2l vand,i+1 , kthe1 link0, 1,lengths, N 푙lv푖+,i+11, 푙푖+2 l (14) are known. Considering that Link l푖O+ 1= whosel + posturek2·∆l , is to be determinedk2 = 0, 1, ··· cannot, Nl collide i O,i Oi Oi with Boundaries 3 and 4, it is easy to select a 푙푣,푖+1 based on the position information of the two boundaries.and At this time, the choice of 푶푖 is the intersection (only part of the intersection is made in the figure) of two spheres whose sphere centers and radii are 푶 , u = k3·∆u, k = 0, 1, ··· , Nu,푖+1 (15) 푶푖+2 and 푙푖+1, 푙푣,푖+1 respectively. Some points (collision points 1 and 2) in the intersection may cause respectively,the link to collide where with∆ Boundariesl , ∆l , 1 and and∆ 2.u Bydenote introducing the value the distance intervals 푙 , of lv,i+1, lO and u, v,i+1 Oi 푂푖 i these pointsrespectively, that cause colli andsionN canl be, eliminated.Nl , and N Therefore,u denote 푙 the푣,푖+1 maximum and 푙푂 jointly of the deter- indices k1, k2, and k3, v,i+1 Oi 푖 mine the collisionrespectively.-free posture (collision-free point) of Link 푖 + 1. Let δi denote the maximum distance from the point on Link i to the link’s axis. If

Boundary 3 Boundary 2 di,min > 2δi, (16) Oi+1 Oi+2 it means that Link i has no collisionli+2 with the boundary. Take a ﬁve-UJ manipulator as an

exampleCollision point to 1 describe the above collision detection analysis process, as shown in Figure5. li+1 By combining the boundarylv,i+1 distribution, and taking the avoidance of collision be- Oi tween the links and the boundary as the criterion, the distances l + and l can be used to Collision-free point v,i 1 Oi describe the manipulator’sCollision overall point 2 conﬁguration in a narrow space scene more intuitively. This is theIntersection reason why the two distances are chosen as the parameters of the inverse kine- Boundary 1 lO,i matics solution process. Take Figure6 as an example, the environment has four boundaries, Boundary 4 and the coordinate system origins Oi+1,Oi+2 and the link lengths li+1, li+2 are known. Con- O0 sidering that Link i + 1 whose posture is to be determined cannot collide with Boundaries 3 and 4, it is easy to select a lv,i+1 based on the position information of the two boundaries. At this time, the choice of Oi is the intersection (only part of the intersection is made in Figure 6. Underthe a ﬁgure) certain boundary of two spheresdistribution, whose the posture sphere of Link centers 푖 + 1 and determ radiiined are by theOi+ dis-1, Oi+2 and li+1, lv,i+1

tances 푙푣,푖+1 respectively.and 푙푂푖 . Some points (collision points 1 and 2) in the intersection may cause the link to

Combining Equations (10) and (11), the determination algorithm of 푙푣,푖+1 and 푙푂푖 can be summarized, as shown in Figure 7. The algorithm successively uses Equations (11)– (16) to gradually narrow the value range. In this process, according to Equations (14) and Appl. Sci. 2021, 11, x FOR PEER REVIEW 10 of 19

Figure 5. Collision detection for a five-general spherical joint (GSJ) manipulator.

By combining the boundary distribution, and taking the avoidance of collision be-

tween the links and the boundary as the criterion, the distances 푙푣,푖+1 and 푙푂푖 can be used to describe the manipulator’s overall configuration in a narrow space scene more intuitively. This is the reason why the two distances are chosen as the parameters of the inverse kinematics solution process. Take Figure 6 as an example, the environment has four boundaries, and the coordinate system origins 푶푖+1,푶푖+2 and the link lengths 푙푖+1, 푙푖+2 are known. Considering that Link 푖 + 1 whose posture is to be determined cannot collide with Boundaries 3 and 4, it is easy to select a 푙 based on the position information of Appl. Sci. 2021, 11, 1277 푣,푖+1 10 of 19 the two boundaries. At this time, the choice of 푶푖 is the intersection (only part of the intersection is made in the figure) of two spheres whose sphere centers and radii are 푶푖+1, 푶푖+2 and 푙푖+1, 푙푣,푖+1 respectively. Some points (collision points 1 and 2) in the intersection 푙 maycollide cause with the Boundaries link to collide 1 and with 2. By Boundaries introducing 1 theand distance 2. By introducinglOi , these points the distance that cause 푂푖, collision can be eliminated. Therefore, l and l jointly determine the collision-free these points that cause collision can be eliminated.v,i+1 Therefore,Oi 푙푣,푖+1 and 푙푂푖 jointly deter- posturemine the (collision-free collision-free point)posture of (collision Link i + 1.-free point) of Link 푖 + 1.

Boundary 2 Boundary 3

Oi+1 li+2 Oi+2

Collision point 1 li+1 lv,i+1 Oi Collision-free point Collision point 2 Intersection

Boundary 1 lO,i Boundary 4

Figure 6. Under a certaincertain boundaryboundary distribution, distribution, the the posture posture of of Link Linki + 푖1+determined1 determined by the bydistances the dis-

tances 푙푣,푖+1 and 푙푂푖 . lv,i+1 and lOi . Appl. Sci. 2021, 11, x FOR PEER REVIEW 푙 11 of푙 19 Combining EquationsEquations (10)(10) and and (11), (11), the the determination determination algorithm algorithm of l ofv,i +1푣,푖and+1 andlOi can 푂푖 canbe summarized, be summarized as shown, as shown in Figure in Figure7. The 7. The algorithm algorithm successively successively uses uses Equations Equations (11)–(16) (11)– (1to6 gradually) to gradually narrow narrow the the value value range. range. In In this this process, process, according according to to Equations Equations (14)(14) andand (15),(15), only only ﬁnite finite number number of of calculations calculations of of explicit explicit equations equations and and conditional conditional judgments judgments of of somesome calculation calculation results results are are performed, performed, which which satisfying satisfying the closed-form the closed-form deﬁnition definition in [24 in]. Therefore,[24]. Therefore, this will this not will affect not affect the closed-form the closed- natureform nature of the of inverse the inverse kinematics kinematics algorithm algo- proposedrithm proposed in this paper.in this paper.

Desired position and orientation of end-effector Link lengths Range of joint angles Constant angles Maximum distance from the point on Link i to its axis

Using (10) to get a rough range of

Using (11)-(14) to get , judging whether Boundary information of they satisfies (15), then saving the distances satisfying (15) narrow environment

Saving the distances satisfying inequalities in (9) to determine the final range

End

Figure 7. Determination algorithm for the distances 푙푣,푖+1 and 푙푂푖 . Figure 7. Determination algorithm for the distances lv,i+1 and lOi .

3.3.3. GSJ Angles for Multiple-GSJ Section Based on a list of locations provided by the distance-based algorithm in Section 3.3.1, 푖−1 the corresponding list of 풂푖 (푖 = 1, ⋯ , 푛) can be obtained through the following derivation: From Equation (4), the component 풂푖 of rotation matrix 푹푖 is given by

풑푖−풑푖−1 풂푖 = . (17) 푙푖

For given 훽1, 훾1, ⋯ , 훽푖−1, 훾푖−1, the component 풂푖 of rotation matrix 푹푖 can also be expressed by

푖−1 풂푖 = 푹푖−1(훽1, 훾1, ⋯ , 훽푖−1, 훾푖−1) ∙ 풂푖 . (18)

Combining Equations (17) and (18) gives

푖−1 −1 풑푖−풑푖−1 풂푖 = (푹푖−1(훽1, 훾1, ⋯ , 훽푖−1, 훾푖−1)) ∙ . (19) 푙푖

Applying the inverse kinematics of single-GSJ section expressed in Equation (6) gives the closed-form solution for GSJ angles. Set the value of 푖 equal to 1, … , 푛 successively, and the solution of (훽1, 훾1, ⋯ , 훽푛, 훾푛) can be computed by Equations (6) and (19). The combination of Equations (6) and (19) provides the closed-form solution for the studied 2푛- DOF serial manipulator with 푛 identical UJs. Appl. Sci. 2021, 11, 1277 11 of 19

3.3.3. GSJ Angles for Multiple-GSJ Section Based on a list of locations provided by the distance-based algorithm in Section 3.3.1, i−1 the corresponding list of ai (i = 1, ··· , n) can be obtained through the following derivation: From Equation (4), the component ai of rotation matrix is given by

pi − pi−1 ai = . (17) li

For given β1, γ1, ··· , βi−1, γi−1, the component ai of rotation matrix Ri can also be expressed by i−1 ai = Ri−1(β1, γ1, ··· , βi−1, γi−1)·ai . (18) Combining Equations (17) and (18) gives

i−1 −1 pi − pi−1 ai = (Ri−1(β1, γ1, ··· , βi−1, γi−1)) · . (19) li Applying the inverse kinematics of single-GSJ section expressed in Equation (6) gives the closed-form solution for GSJ angles. Set the value of i equal to 1, ... , n successively, and the solution of (β1, γ1, ··· , βn, γn) can be computed by Equations (6) and (19). The combination of Equations (6) and (19) provides the closed-form solution for the studied 2n-DOF serial manipulator with n identical UJs. Next, the closed-form property of the proposed inverse kinematics solution is analyzed. The closed-form is determined by two parts, i.e., the determination of the two distances and the resolution of GSJ angles for multiple-GSJ section. Using a ﬁnite number of standard operations [24], Equation (14) gives the closed-form solution of the two distances after judging whether they satisfy the constraints in Equations (10) and (16). This ensures the ﬁrst part’s closed-form property. The closed-form of the inverse kinematics solution formed by Equations (6), (9), and (19) is obvious. Hence, closed-form property of the proposed inverse kinematics solution for the 2n-DOF hyper-redundant manipulator is ensured.

4. Simulation In this simulation, the parameters of the 2n-DOF manipulator are shown as follows: the number of UJ n = 5, the link lengths li = 1.0, i = 1, ··· , 5, and the GSW angles ◦ ◦ αi = 9 , and βi, γi (i = 1, ··· , 5) with bounds “not more than 25 and not less than −25◦”. For this manipulator, the distances introduced in the proposed closed-form inverse

kinematics includes lv,4, lO3 , lv,3, lO2 , lv,2, lO1 . Based on Equation (11), these introduced distances have the following value ranges got by using the joint angle bounds: 0.6282 ≤ ≤ ≤ ≤ ≤ ≤ ≤ ≤ ≤ ≤ lv,4 2, 2.6431 lO3 3, 0.6282 lv,3 2, 1.9086 lO2 2, 0.6282 lv,2 2, = = lO1 l1 1.0. In addition, these distances also satisfy the following inequality constraints ≥ − 6= given by Equation (10): ∆ lv,4, lO3 0 and p5,x p4,y p5,y p4,x 0 during the process ≥ − 6= of computing p3; ∆ lv,3, lO2 , lv,4, lO3 0 and p4,x p3,y p4,y p3,x 0 during the process ≥ − 6= of computing p2; ∆ lv,2, lO1 , lv,3, lO2 , lv,4, lO3 0 and p3,x p2,y p3,y p2,x 0 during the process of computing p1. To verify the validity of the proposed closed-form inverse kinematics model, the solution for a random single given pose, the straight-line paths along the x-axis, y-axis, and z-axis of the base frame, and the circle path are solved through using the proposed closed-form inverse kinematics. The tool used to perform the simulation is an open source software called GNU Octave.

4.1. Solutions for a Random Single Given Pose For a random given pose of the end-effector within the workspace of the 5-UJ manipu- 0 T 0 T lator, for example, n5 = 0.7277 0.6755 0.1192 , s5 = −0.6328 0.5940 0.4967 , 0 T 0 T a5 = 0.2647 −0.4369 0.8597 , p5 = 0.5476 −1.6509 4.5615 , it is in the nar- Appl. Sci. 2021, 11, 1277 12 of 19

row space whose boundary is a cylindrical surface with a radius of 0.55. By using Equations = = = = = (10), and (12)–(16) with the selected intervals ∆lv,4 ∆lv,3 ∆lv,2 0.001, ∆lO3 ∆lO2 Appl. Sci. 2021, 11, x FOR PEER REVIEW 0.001, and ∆ = 0.001 and the maximum envelope distances δ = ··· = δ = 0.2, the range13 of 19 u 1 5 of these distances lv,4, lO3 , lv,3, lO2 , lv,2 can be further reduced. The reduced value range is presented in the form of a combination of these ﬁve distances, as shown in Figure8. As

it can be seen, the value ranges of lv,4, lO3 , lv,3, lO2 , lv,2 have great decrease comparing to the ones computed by Equation (11) and are close to their respective upper bonds. This Moreover, accordingis becauseto Figure that the9, the smaller configuration value of these distancess of link indicates 5 remain the larger unchanged joint angles andbecause of the existence of thesatisfying general the spherical need of harmony wrist, between i.e., thesethe 5th distances general to make spherical all joint angles joint. within Simulta- neously, the solutiontheir changes limits is necessary.in law Inwith fact, the function change of Figure of the8 is distance to provide the of initial 푙 . estimate for the value of these distances. 푣,4

Figure 8. CombinationsFigure 8. Combinations of introduced of introduced distances distances satisfying satisfying the constraints the const of Equationsraints (10)–(16).of Equations (10)–(16).

To show multiple solution for a given pose, different value of lv4 and some valid

values of lO3 , lv,3, lO2 , lv,2 within the value ranges shown in Figure8 are chosen to solve the above given pose using the proposed closed-form inverse kinematics. The value of

lv,4 changes from 1.9573 to 1.9989 at 0.001 interval; the value of lO3 , lv,3, lO2 , lv,2 are equal to 2.9249, 1.9706, 1.9730, and 1.9690, respectively. The configuration of the manipulator plotted through using the forward kinematics is shown in Figure9a. The corresponding joint angles are shown in Figure9b. For some given target pose, different manipulator configurations can be provided by choosing different distances, for example, lv,4. This can be applied in obstacle avoidance. Moreover, according to Figure9, the configurations of link 5 remain unchanged because of the existence of the general spherical wrist, i.e., the 5th general spherical joint. Simultaneously, the solution changes in law with the change of the distance of lv,4.

(a)

(b) Figure 9. Solution for a random given pose within workspace: (a) configuration of the manipulator; (b) corresponding joint angles. Appl. Sci. 2021, 11, x FOR PEER REVIEW 13 of 19

Moreover, according to Figure 9, the configurations of link 5 remain unchanged because of the existence of the general spherical wrist, i.e., the 5th general spherical joint. Simulta- neously, the solution changes in law with the change of the distance of 푙푣,4.

Appl. Sci. 2021, 11, 1277 13 of 19 Figure 8. Combinations of introduced distances satisfying the constraints of Equations (10)–(16).

(a)

(b)

Figure 9. SolutionFigure 9. Solution for a random for a random given given pose pose within within workspace: workspace: (a(a)) conﬁguration configuration of the of manipulator; the manipulator; (b) corresponding (b) corresponding joint angles.joint angles. 4.2. Solutions for the Straight-Line Paths along the x-axis, y-axis, and z-axis of the Base Frame Further, to verify the validity of the proposed closed-form inverse kinematics, the straight-line paths along the x-axis, y-axis, and z-axis of the base frame are regarded as test objects and the solution of these paths are solved by applying the proposed closed-form inverse kinematics. For the straight-line path along the x-axis started at the above given pose, all of the distances are equal to valid value, which are 1.9573, 2.9249, 1.9706, 1.9730, and 1.9690, respectively. The conﬁgurations of the manipulator plotted through using the forward kinematics is shown in Figure 10a. The corresponding joint angles is shown in Figure 10b. For the straight-line path along the y-axis started at the above given pose, all of the distances are equal to valid value, which are 1.9573, 2.9249, 1.9706, 1.9730, and 1.9690, respectively. For the straight-line path along the z-axis started at the above given pose, all of the distances are equal to valid value, which are 1.9633, 2.9249, 1.9706, 1.9730, and Appl. Sci. 2021, 11, x FOR PEER REVIEW 14 of 19

4.2. Solutions for the Straight-Line Paths along the x-axis, y-axis, and z-axis of the Base Frame Further, to verify the validity of the proposed closed-form inverse kinematics, the straight-line paths along the x-axis, y-axis, and z-axis of the base frame are regarded as test objects and the solution of these paths are solved by applying the proposed closed- form inverse kinematics. For the straight-line path along the x-axis started at the above given pose, all of the distances are equal to valid value, which are 1.9573, 2.9249, 1.9706, 1.9730, and 1.9690, respectively. The configurations of the manipulator plotted through using the forward kinematics is shown in Figure 10a. The corresponding joint angles is shown in Figure 10b. For the straight-line path along the y-axis started at the above given pose, all of the distances are equal to valid value, which are 1.9573, 2.9249, 1.9706, 1.9730, and 1.9690, respectively. For the straight-line path along the z-axis started at the above given pose, all of the distances are equal to valid value, which are 1.9633, 2.9249, 1.9706, Appl. Sci. 2021, 11, 1277 1.9730, and 1.9690, respectively. The results for the straight-line paths along14 of 19 the y-axis and z-axis are shown in Figures 11 and 12, respectively. According to Figures 10b,11b and 12b, the solution changes continuously with the 1.9690, respectively. The results for the straight-line paths along the y-axis and z-axis are change shownof coordinate, in Figures 11 while and 12 the, respectively. five distances used remain the same.

(a)

(b)

Figure 10. SolutionFigure for 10. theSolution straight for the-line straight-line path along path alongthe x the-axis:x-axis: (a) (configurationa) conﬁguration of of the the manipulator; manipulator; (b) corresponding (b) corresponding joint angles. joint angles. Appl. Sci. 2021, 11Appl., x FOR Sci. 2021 PEER, 11, 1277REVIEW 15 of 19 15 of 19

(a)

(b)

Figure 11. SolutionFigure for 11. Solutionthe straight for the-line straight-line path al pathong along the y the-axis:y-axis: (a) ( aconfiguration) conﬁguration of of the the manipulator; manipulator; (b) corresponding (b) corresponding joint angles. joint angles.

(a) Appl. Sci. 2021, 11, x FOR PEER REVIEW 15 of 19

(a)

(b) Appl. Sci. 2021, 11, 1277 16 of 19 Figure 11. Solution for the straight-line path along the y-axis: (a) configuration of the manipulator; (b) corresponding joint angles.

Appl. Sci. 2021, 11, x FOR PEER REVIEW 16 of 19

(a)

(b)

Figure 12.Figure Solution 12. Solutionfor the straight-line for the straight-line path pathalong along the thez-axis:z-axis: (a) ( aconfiguration) configuration of thethe manipulator; manipulator; (b) ( correspondingb) corresponding joint angles. joint angles. According to Figures 10b, 11b and 12b, the solution changes continuously with the 4.3. Solutionchange of for coordinate, a Circle Path while the five distances used remain the same. After4.3. Solution the above for a Circle test Pathfor the line path, the circle path is also tested using the proposed closed-formAfter inverse the above kinematics. test for the line This path, circle the circlepath pathpasses is also through tested using the theend-effector proposed position shownclosed-form in Figure inverse 9, and kinematics. the projection This circle of this path circle passes path through in thethe end-effector 푥푦 plane positionis a circle whose originshown is (0 in Figure 0 0)9., For and this the projection circle path, of this all circleof the path distances in the xy areplane equal is a to circle valid whose value, which origin is 0 0 0 . For this circle path, all of the distances are equal to valid value, are 1.9573,which are2.9249, 1.9573, 1.9706, 2.9249, 1.9730, 1.9706, 1.9730, and 1.9690, and 1.9690, respectively. respectively. The The configuration configuration of of the the manip- ulatormanipulator plotted through plotted throughusing the using forward the forward kinematics kinematics is is shown shown inin Figure Figure 13 a.13a. The The corre- spondingcorresponding joint angles joint anglesis shown is shown in Figure in Figure 13b. 13b.

(a)

(b) Appl. Sci. 2021, 11, x FOR PEER REVIEW 16 of 19

(b) Figure 12. Solution for the straight-line path along the z-axis: (a) configuration of the manipulator; (b) corresponding joint angles.

4.3. Solution for a Circle Path After the above test for the line path, the circle path is also tested using the proposed closed-form inverse kinematics. This circle path passes through the end-effector position shown in Figure 9, and the projection of this circle path in the 푥푦 plane is a circle whose origin is (0 0 0). For this circle path, all of the distances are equal to valid value, which are 1.9573, 2.9249, 1.9706, 1.9730, and 1.9690, respectively. The configuration of the manip- Appl. Sci. 2021, 11, 1277 ulator plotted through using the forward kinematics is shown in Figure 13a.17 of 19 The corresponding joint angles is shown in Figure 13b.

(a)

(b)

Figure 13. Solution for a circle path: (a) conﬁguration of the manipulator along the circle path; (b) corresponding joint angles.

5. Discussion As expected by the determination algorithm for the distances shown in Figure7, for the desired end effector pose, Figure8 shows all of the distance combinations that meet the constraints described by Equations (10)–(16). Each set of combinations corresponds to a set of inverse kinematics solutions. By using the proposed closed-form inverse kinematics in Equations (6), (9), and (19), Figure9 shows different manipulator conﬁgurations and joint angles corresponding to different distance combinations. The end-effector poses of these conﬁgurations remain unchanged. Simultaneously, the calculated joint angles are within the given joint limit “not more than 0.4363 rad (25 ◦) and not less than −0.4363 rad (−25 ◦)”. These results verify the correctness of the proposed inverse kinematics model. Moreover, compared with the inverse kinematics based on backbone curve in [10], the proposed model is completely closed-form. Compared with the algorithm based on Jacobian analysis in [23], the proposed method can solve all joint angles at once. Compared with the inverse kinematics model proposed in [22] for three universal joints and one rotary joint manipulator, the proposed algorithm has stronger applicability. Using a set of constant distance combinations, Figure 10 shows the inverse kinematics solution when the end-effector moves linearly along the x axis. Figures 11 and 12 show the corresponding results for the y- and z-axes, respectively. Figure 13 shows the inverse kinematics solution when the end effector performs circular motion. These joint angles are all within the given joint limit. These results further verify the correctness of the proposed Appl. Sci. 2021, 11, 1277 18 of 19

inverse kinematics model. Simultaneously, these results also indicate that the distance combination has the following property: a set of distance combinations can be used to solve the inverse kinematics of different end-effector poses. These distances have geometric meanings as shown in Figure6, so this property has certain guiding signiﬁcance for path planning and obstacle avoidance of the 2n-DOF hyper-redundant manipulator.

6. Conclusions In this paper, a novel method to derive closed-form solution for the inverse kinematics problem of 2n-DOF redundant serial manipulators is proposed. This method is based on a novel concept named as general spherical wrist and a distance-based formula for the positions of the universal joints. It is noteworthy that the proposed general spherical joint reveals the essence of the coordinate transformation between the two link frames connected by universal joint, and it becomes an alternative joint that can realize decoupling of the positions and orientations after the spherical wrist. A determination algorithm for the introduced distances is proposed, which is the prerequisite for realizing the distance-based algorithm. Unlike other methods found in literature, the method exposed in this work does not involve any numerical optimization, and is, therefore, rigorous analytically. Moreover, this method is extremely geometrically intuitive and is easy to understand. The closed-form property of the proposed inverse kinematics solution is justiﬁed by analyzing closed-form properties of the determination of the two distances and the resolution of GSJ angles for multiple-GSJ section. The simulation results with single given pose of end-effector, straight- line path along three coordinate axes of the base frame, and a circle path verify the validity and effectiveness of the proposed closed-form solution. Hence, the proposed solution can be used in trajectory planning and real-time control of the redundant manipulator. Moreover, according to the simulation results, a distance combinations’ characteristic of “a set of distance combinations can be used to solve different end-effector poses” was found. Based on this characteristic, further research will focus on the path planning and obstacle avoidance of the 2n-DOF hyper-redundant manipulator. The inverse Jacobian kinematics based on the closed-form solution proposed in this work is another research interest.

Author Contributions: Y.L. and P.Q. developed the methodology; Y.L. and P.Q. conceived and designed the simulations; Y.L. and H.L. wrote the original draft of the paper; S.D. and D.W. reviewed and edited the paper. All authors have read and agreed to the published version of the manuscript. Funding: This research received no external funding. Institutional Review Board Statement: Not applicable. Informed Consent Statement: Not applicable. Data Availability Statement: This paper is no data. Acknowledgments: The authors thank Tian Lan and Xiaolong Liu for the helpful suggestions. Conﬂicts of Interest: The authors declare no conﬂict of interest.

References 1. Hussain, R.; Qureshi, A.; Mughal, R.A.; Ijaz, R.; Rashid, N.; Tiwana, M.I.; Iqbal, J. Inverse kinematics control of redundant planar manipulator with joint constraints using numerical method. In Proceedings of the 2015 15th International Conference on Control, Automation and Systems (ICCAS), Busan, Korea, 13–16 October 2015; pp. 806–810. 2. Du, H.B.; Zhao, Y.W.; Li, X.G.; Han, J.D.; Wang, Z.; Song, G.L. A closed-loop framework for inverse kinematics of the 7-DOF manipulator. In Proceedings of the 2nd International Conference on Control Science and Systems Engineering, Singapore, 27–29 July 2016; pp. 237–241. 3. Deepak, T.; Ambarish, G.; Norman, B. Real-time inverse kinematics techniques for anthropomorphic limbs. Graph Models 2000, 62, 353–388. 4. Omisore, M.O.; Han, S.; Ren, L.; Zhang, N.; Wang, L. Noniterative geometric approach for inverse kinematics of redundant leadmodule in a radiosurgical snakelike robot. Biomed. Eng. Online 2017, 16, 93. [CrossRef][PubMed] Appl. Sci. 2021, 11, 1277 19 of 19

5. Isiah, Z.; Luis, B. A novel closed-form solution for the inverse kinematics of redundant manipulators through workspace analysis. Mech. Mach. Theory 2018, 121, 829–843. 6. Pieper, D. The Kinematics of Manipulation under Computer Control. Ph.D. Thesis, Stanford University, Stanford, CA, USA, 1968. 7. Srinivas, N.; Matthew, A.C.; Bryan, A.J.; Walker, I.D. Closed-form inverse kinematics for continuum manipulators. Adv. Robot. 2009, 23, 2077–2091. 8. Pfurner, M. Closed form inverse kinematics solution for a redundant anthropomorphic robot arm. Comput. Aided Geom. D 2016, 47, 163–171. [CrossRef] 9. Kai, X.; Wukun, M.; Zhixiong, Y.; Han, L.; Zhu, X. Design of a hyper-redundant continuum manipulator for intra-cavity tasks. In Proceedings of the 2014 IEEE International Conference on Robotics and Biomimetics, Bali, Indonesia, 5–10 December 2014; pp. 380–385. 10. Wenfu, X.; Zonggao, M.; Tianliang, L.; Liang, B. A modiﬁed modal method for solving the mission-oriented inverse kinematics of hyper-redundant space manipulators for on-orbit servicing. Acta Astronaut. 2017, 139, 54–66. 11. Chirikjian, G.; Burdick, J. A modal approach to hyper-redundant manipulator kinematics. IEEE Trans. Robot. Autom. 1994, 10, 343–354. [CrossRef] 12. Goldenberg, A.A.; Benhabib, B.; Fenton, R.G. A complete generalized solution to the inverse kinematics of robots. IEEE J. Robot. Autom. 1985, 1, 14–20. [CrossRef] 13. Sugihara, T. Solvability-Unconcerned Inverse Kinematics by the Levenberg–Marquardt Method. IEEE Trans. Robot. 2011, 27, 984–991. [CrossRef] 14. Wang, L.-C.; Chen, C. A combined optimization method for solving the inverse kinematics problems of mechanical manipulators. IEEE Trans. Robot. Autom. 1991, 7, 489–499. [CrossRef] 15. Li, Y.; Leong, S.H. Kinematics control of redundant manipulators using a CMAC neural network combined with a genetic algorithm. Robotica 2004, 22, 611–621. [CrossRef] 16. Yin, F.; Wang, Y.; Yang, Y. Inverse kinematics solution for robot manipulator based on neural network under joint subspace. Int. J. Comput. Commun. 2012, 7, 459–472. 17. Beheshti, M.T.H.; Tehrani, A.K.; Ghanbari, B. An optimized adaptive fuzzy inverse kinematics solution for redundant manipulators. In Proceedings of the 2003 IEEE International Symposium on Intelligent Control, Houston, TX, USA, 5–8 October 2003; pp. 924–929. 18. Kesheng, W. Application of genetic algorithms to robot kinematics calibration. Int. J. Syst. Sci. 2009, 40, 147–153. 19. Ananthanarayanan, H.; Ordóñez, R. Real-time inverse kinematics of (2n+1) DOF hyper-redundant manipulator arm via a combined numerical and analytical approach. Mech. Mach. Theory 2015, 91, 209–226. [CrossRef] 20. Kim, J.S.; Jeong, J.H.; Park, J.H. Inverse kinematics of a redundant manipulator based on conformal geometry using geometric approach. In Proceedings of the 2015 12th International Conference on Informatics in Control, Automation and Robotics (ICINCO), Colmar, France, 21–23 July 2015; pp. 179–185. 21. Kireanski, M.V.; Petrovie, T.M. Combined analytical-pseudoinverse inverse kinematic solution for simple redundant manipulators and singularity avoidance. Int. J. Robot. Res. 1993, 12, 188–196. [CrossRef] 22. Huang, H.; Dong, E.; Xu, M.; Yang, J.; Low, K.H. Mechanism design and kinematic analysis of a robotic manipulator driven by joints with two degrees of freedom (DOF). Ind. Robot. 2018, 45, 34–43. [CrossRef] 23. Tang, L.; Wang, J.; Zheng, Y.; Gu, G.; Zhu, L.; Zhu, X. Design of a cable-driven hyper-redundant robot with experimental validation. Int. J. Adv. Robot. Syst. 2017, 14, 172988141773445. [CrossRef] 24. Borwein, J.M.; Crandall, R.E. Closed forms: What they are and why we care. N. Am. Math. Soc. 2013, 60, 50–65. [CrossRef]