Motion-Planning Algorithm for a Hyper-Redundant Manipulator in Narrow Spaces

: In this study, a hyper-redundant manipulator was designed for detection and searching in narrow spaces for aerospace and earthquake rescue applications. A forward kinematics equation for the hyper-redundant manipulator was derived using the homogeneous coordinate transformation method. Based on the modal function backbone curve method and the known path, an improved modal method for the backbone curves was proposed. First, the configuration of the backbone curve for the hyper-redundant manipulator was divided into two parts: a mode function curve segment of the mode function and a known path segment. By changing the discrete points along the known path, the backbone curve for the manipulator when it reached a specified path point was dynamically obtained, and then the joint positions of the manipulator were fitted to the main curve by dichotomy. Combined with engineering examples, simulation experiments were performed using the new algorithm to extract mathematical models for external narrow space environ-ments. The experimental results showed that when using the new algorithm, the hyper-redundant manipulator could complete the tasks of passing through curved pipes and moving into narrow workspaces. The effectiveness of the algorithm was also proven by these experiments.


Introduction
Hyper-redundant manipulators have tremendous application potential for complex working environments and narrow spaces, and currently play important roles in aerospace engineering [1,2], nuclear power engineering [3][4][5], and clinical medicine [6][7][8][9]. To maintain high flexibility and portability, hyperredundant manipulators often adopt rope drives [10]. Owing to the many degrees of freedom in hyperredundant manipulators, solving their inverse kinematics equations is usually more complicated. There are three primary method types commonly used for solving inverse kinematics [11]. One method is to solve the differential kinematics equations numerically using a pseudo-inverse solution of the Jacobian matrix [12,13]. This method involves a large number of calculations and has a slow calculation speed. Another method is to solve the inverse kinematics of a manipulator using an artificial neural network [14,15]; however, the training set for the neural network must be very large, and it is difficult to achieve instantaneous kinematics planning for the manipulator. The geometric method, based on the backbone curve proposed by Chirikjian et al. [16,17], is also useful for solving manipulator inverse kinematics. This method requires a small number of calculations and can be used for instantaneous control and online path planning.
Path-following motion [18] refers to motion where the end of a hyper-redundant manipulator follows a specified path, reproducing the path curve as closely as possible during the motion. During motion planning for a hyper-redundant manipulator with a mobile base platform, path-following motion is more aligned with its motion characteristics and can achieve the necessary flexibility and freedom. Fuzzy control methods can be used to achieve this kind of motion planning [19,20]. Additionally, for complex three-dimensional models, principal component analysis (PCA) can be used to reduce the dimensionality of the model, project the data into a low-dimensional subspace, and reduce the data processing complexity [21].
In this study, a hyper-redundant manipulator was designed, and its positive kinematics equation was derived by a homogeneous matrix coordinate transformation. Focusing on the problems that path-following motion is only suitable for a hyper-redundant manipulator with a moving base and that the mode function backbone curve is not suitable for moving along the tangent direction of a known path, a cost-effective method is proposed to achieve motion planning for a hyper-redundant manipulator based on a combination of the backbone curve and the path-following concept. The algorithm caused the position of the end effector to move along the tangent direction of a known path, and the joint position of the entering path was always close to the known path. This algorithm allows hyper-redundant manipulators to complete the motion in pipelines or other narrow workspaces.

Positive Kinematics of a Hyper-Redundant Manipulator
A hyper-redundant manipulator is shown in Fig. 1. This manipulator was composed of 12 links and an end effector. The links were connected by 12 universal joints, the hyper-redundant manipulator was driven by ropes, and driving motors were located in the manipulator box. The structural and joint angle parameters of the hyper-redundant manipulator are shown in Tab. 1. The joints in the manipulator are characterized by θ i and α i , where θ i , α i ∈ (−60 • , 60 • ) and i = 1, . . . , 12 O i represents the position of the center of each universal joint and the origin of each coordinate system. All the coordinate systems are shown in Fig. 1. The x 0 direction in the coordinate system of the base was defined as the direction from the center of the first universal joint to the position of the end effector when all the joints had angles of 0 o . The z 0 direction in the coordinate system of the base was defined as the vertical upward direction, and the y 0 direction in the coordinate system of the base was determined by the right-hand rule. A homogeneous coordinate transformation was used to obtain the positive kinematics model [22], and an O i x i y i z i coordinate system was established at each universal joint. The origin of the O 1 x 1 y 1 z 1 coordinate system coincided with the origin of the O 0 x 0 y 0 z 0 coordinate system. The positive direction of each z i axis pointed to the center of the next universal joint. The direction of the yawing motion for each universal joint was selected as the x i axis and the direction of the pitching motion was chosen as the y i axis for each joint. The upward direction of each x i axis was positive, and the y i axis was determined by the right-hand rule, as shown in Fig. 2a  The homogeneous transformation matrix used during this study is presented as where c and s represent cos and sin, respectively.
The coordinate transformation process from the basic O 0 x 0 y 0 z 0 coordinate system to the O 0 x 1 y 1 z 1 coordinate system is to rotate the O 0 x 0 y 0 z 0 coordinate system by −90 • around the y 0 axis to obtain the O 0 x 0 y 0 z 0 coordinate system, then the O 0 x 0 y 0 z 0 coordinate system must rotate 180 • around the x 0 axis to obtain the O 0 x 0 y 0 z 0 coordinate system. Then the O 0 x 0 y 0 z 0 coordinate system must be rotated around the y 0 axis by α 1 to obtain the O 0 x 0 y 0 z 0 coordinate system. Finally, the O 0 x 0 y 0 z 0 coordinate system is rotated around the x 0 axis by θ 1 to obtain the O 0 x 1 y 1 z 1 coordinate system, as shown in Fig. 3. The homogeneous transformation matrix from the basic O 0 x 0 y 0 z 0 coordinate system to the O 0 x 1 y 1 z 1 coordinate system can be expressed by The transformation matrix of the i th joint is expressed by The kinematics equation for the hyper-redundant manipulator can then be expressed by ⎡ When the joint angles are inserted into Eqs. (1)-(4), the position of the manipulator's end effector can be obtained.

Motion-Planning Algorithm Based on the Backbone Curve and Path-Following
The geometric method, based on the backbone curve, was used to solve the inverse kinematics of the hyper-redundant manipulator in this study. The backbone curve of the hyper-redundant manipulator had to be determined first. Then the positions of the hyper-redundant manipulator joints must be fitted to the backbone curve. Finally, the geometric method was used to solve for the joint angles.

Backbone Curve Based on the Mode Function
The modal method uses differential geometry to solve the inverse kinematics of hyper-redundant manipulators. The backbone curve of the mode function is shown in Fig. 4. The mode function of the backbone curve [23] is expressed by where s represents the normalized length parameter of the backbone curve and l represents the length of the backbone curve. The coordinates of each point in the backbone curve in the O 0 x 0 y 0 z 0 coordinate system are represented by x (s, t), y (s, t), and z (s, t). u(σ , t) represents the unit tangent vector of the backbone curve at σ , and can be expressed by In Eq. (6), K(s, t) and T(s, t) determine the pose of the backbone curve and are expressed by In Eq. (7), a 1 , a 2 , and a 3 are modal participation factors. b 1k and b 1t are the two angles of the initial position of the main curve corresponding to K(0, t) and T(0, t), respectively. b 2k and b 2t are the two angles of the end position of the main curve corresponding to K(1, t) and T(1, t), respectively. b 1k , b 1t , b 2k , and b 2t were determined by the desired angles at the beginning and end of the backbone curve. When solving for the direction vector, u(s, t), the Newton iteration method was used to obtain the modal participation factors, a 1 , a 2 , and a 3 : In Eq. (8), ω is a constant that controls the convergence speed, m is the number of iterations, and X D is the target end position point. X m is the end point vector of the mode function backbone curve obtained by substituting a m into Eq. (7) after the m th iteration, and J −1 a (a m , 1) is the inverse matrix of the 3 × 3 modal Jacobian matrix when the ridge arc length, s, is equal to 1. The element relationship in J a (a, 1) is presented as

New Algorithm
The motion-planning algorithm for the hyper-redundant manipulator based on path-following and the backbone curve is primarily useful for passing the end effector through curved pipes and narrow spaces.
As shown in Fig. 5, the backbone curve was divided into a mode function curve segment and a known path curve segment. Point O represents the center of the first universal joint and theÃB curve is a path through a narrow space. The curve in the narrow space is defined as a known path segment (curvesĀE andĀE ) and the curve from the base of the manipulator to the initial point of the known path segment is defined as the mode function curve segment (curvesȎSA,ȎTA, andȮUA). The known path length was obtained by the trapezoidal integration method, expressed by In Eq. (10), the integral's upper limit is s = 1. Five primary steps were used to solve for the set of backbone curves in the motion process.
Step 1: The total length of the initial mode function curve,ȎSA, was defined as the total length of the hyper-redundant manipulator, L sum . The starting point of the mode function curve was set as the center of the first universal joint, and end point coincided with the initial point of the known path. The angles b 1k and b 1t at the starting point of the mode function curve were determined by the initial orientation of the manipulator. The two end angles, b 2k and b 2t , remained tangent to the known path, AB, so that the improved backbone curve was always smooth. The backbone curve was obtained using the method described in Section 3.1. The step length and step number were set as λ and N, respectively. Step 2: At the i th step, the backbone curve was composed of the new mode function curve segment, OTA, and the known path curve segment,ĀE , that had been traversed. The length of the known path curve segment that had been traversed was iλ. The new mode function curve segment length, L m , can be expressed by In Eq. (11), α step is generally 1, but when the curvature is large it is equal to 0.98.
Step 3: If the end-actuator position, E , fell between two adjacent given points, C (x C , y C , z C ) and D (x D , y D , z D ), in the known path,ÃB, then linear interpolation was used to determine the position of the landing point, E (x E , y E , z E ). If the Euclidean distance between points C and D was |CD|, then Eq. (10) was used to find the length, L Ù AC , of curve |AC|, and E (x E , y E , z E ) could be obtained from Eq. (12). If E fell on a given point of the known path, then this point was point E . Then the mode function curve,ȎTA, and curveĀE were connected to obtain the backbone curve,˚ OTAE , at the i th step.
Step 4: i = i + 1 was applied, and the process was repeated, beginning with Step 2, to find the backbone curve for each step. The loop was terminated when i = N − 1.
Step 5: When i = N, the step length was changed so that the position, E , of the manipulator's end actuator fell on the end point, B, of the known curve. Then the backbone curve could be obtained.

Fitting of the Hyper-Redundant Manipulator and the Backbone Curve
As shown in Fig. 6, a dichotomy was used to fit the end point of the manipulator and the center of each universal joint to the backbone curve in turn, in the direction from the end point to the origin. The fitting process followed four primary steps.
The initial state of the backbone curve The movement state of the backbone curve The final state of the backbone curve Figure 6: The relationship between the manipulator and the backbone curve during motion Step 1: The initial mode function curve from the base coordinate origin of the manipulator to the starting point of the known path was composed of Q + 1 known discrete points. The initial point of the backbone curve was defined as the left end point of the dichotomy iteration, and the end of the backbone curve as the right end point. The coordinates of the m th point were found, and this point was defined as point W (m = [Q/2]). When the distance from point W to the right end of the backbone curve was greater than the length of the end linkage, the right endpoint remained unchanged, and point W was taken as the left endpoint. When the distance from point W to the right end of the backbone curve was less than l12, the left endpoint remained unchanged, and point W was taken as the right endpoint. Then iterations continued with the new left and right endpoints. The iterations stopped when the distance from point W to the end point of the backbone curve was equal to the length of the end linkage within the accuracy range. If point W coincided with a known point, then this point was taken as the center point of universal joint O 12 . Otherwise, the two known points, P j−1 and P j , that were adjacent to the target point, were found.
Step 2: P j−1 and P j were connected to place the end point of the backbone curve at the center of a circle with l 12 as the arc radius. The intersection of the arc and line P j−1 P j was taken as O 12 , as shown in Fig. 7. Step 3: Similarly, Steps 1 and 2 were repeated to fit all but the first two universal joints to the backbone curve in turn.
Step 4: When determining the position of the second universal joint, the middle vertical plane of vector −−→ O 1 O 3 was used to draw a sphere with O 1 as the center and l 1 as the radius. Circle T resulted from the intersection of the sphere and the plane. Any point on the circle was selected as O 2 . The fitting of the manipulator and the backbone curve is shown in Fig. 8. When a hyper-redundant manipulator is fitted, the length of the backbone curve must be appropriately greater than the length of the manipulator if the backbone curvature is large.

Solving for the Joint Angles
The universal joint angles for the hyper-redundant manipulator were solved using the closed vector method. In Fig. 9, O n+1 represents the endpoint of the end effector. L 1 , L 2 , . . . , L n denote the linkage vectors in the base coordinate system, O 0 x 0 y 0 z 0 . X 2 , X 3 , . . . , X n represent the joint center vectors in the base coordinate system, O 0 x 0 y 0 z 0 , expressed as X i = [x i y i z i 1] T , and X n+1 represents the endpoint vector. The process of solving for the manipulator's joint angles was divided into three primary steps [23]. 4826 CMC, 2022, vol.72, no.3 Step 1: For the first universal joint, the known vector, X 2 , could be expressed by In Eq. (13), x 2 , y 2 , and z 2 are the coordinates of the center position of the second universal joint in the base coordinate system, O 0 x 0 y 0 z 0 , and T 1 0 is the homogeneous transformation matrix shown in Eq. (2). By solving T 1 0 , the rotation angles for the first universal joint, α 1 and θ 1 , were obtained.

Engineering Experiments 4.1 Motion Through a Bent Pipe
As shown in Fig. 10, during the exploration of a certain piece of aviation equipment, the base of a hyper-redundant manipulator was at O 0 (0, 0, 0), and A (1, 200, 100, 300 mm) was the starting point of the central axis of the bent pipe. The diameter of the pipe was 70 mm, the turning radius was 100 mm, and the end point of the central axis of the pipe was located at B (1, 800, 100, 100 mm). The red line between A and B represents the known path segment. The hyper-redundant manipulator was required to pass through the bent pipe. The inverse solution for the hyper-redundant manipulator was solved using the proposed algorithm. The origin of the mode function curve was at O 0 and the end point of the mode function curve was at A. Using the algorithm described in Section 3.1, the four angles, b 1k , b 1t , b 2k , and b 2t , were set as 0, 0, π/2, and 0, and L sum was set as 2,007 mm. The mode function curve segment is shown in Fig. 11a. Also, the end effector of the hyper-redundant manipulator was made to move forward along the curved path with a 5-mm step length. The orientation of the backbone curve at the 60 th step is shown in Fig. 11b. The end point of the backbone curve had traveled 300 mm along the known path. The starting point, the end point, and the four angle parameters, b 1k , b 1t , b 2k , and b 2t , of the mode function curve were the same as in Fig. 11a. In Fig. 11c, the end point of the backbone curve moved to the target position. When solving for the backbone curve at each step, the hyper-redundant manipulator and the backbone curve were fitted by the algorithm described in Section 3.3, and the results are shown in Fig. 12. The three hyper-redundant manipulator orientations in Fig. 12 correspond to the three backbone curves in Fig. 11, and the manipulator orientations were consistent with the backbone curves. In Fig. 12a, the manipulator was in the initial motion state. In Fig. 12b, the end effector and the 11 th and 12 th universal joints were located on the known path and moved along the direction tangent to the path. Fig. 12c shows that the end effector coincided with the end point B, and that the 9 th -12 th universal joints were close to the known path to achieve the expected motion. The inverse solutions are shown in Fig. 13. The red and blue curves in Fig. 13 represent the changes in α and θ , respectively. The trends were relatively smooth. These solutions were substituted into the positive kinematics equation in Section 3.1 to drive the robot's motion. The hyper-redundant manipulator passed through the bent pipe, verifying the effectiveness of the algorithm.

Motion When Entering Narrow Cabins
As shown in Fig. 14, during the process of exploring a piece of an aviation equipment, the center of the first universal joint in the hyper-redundant manipulator was at O 0 (0, 0, 0), and it had to enter the cabins sequentially for cleaning or searching. The manipulator was directed to move along the red path to point C (1,470, −50, 200 mm), and the coordinates of A and B were (1,200, 100, 300 mm) and (1,470,190, 300 mm), respectively. Using the new method proposed in this study, a series of results were generated. A backbone curve was obtained and is shown in Fig. 15a. The orientation of the backbone curve at the 60th step is shown in Fig. 15b. The length of the mode function curve segment was 1,707 mm. The modal participation factors of the mode function curve were regenerated using Equation (9), and the other parameters remained unchanged. In Fig. 15c, the manipulator had reached the target point. Fig. 15d and Fig. 15e represents the backbone curve during the return motion and the backbone curve when the manipulator had returned to the starting point, respectively. The hyper-redundant manipulator and the backbone curve were fitted by the algorithm from Section 3.3, and the results are shown in Fig. 16. The five orientations of the hyper-redundant manipulator in Fig. 16 correspond to the five backbone curves in Fig. 15, and the manipulator orientations were consistent with the backbone curves. The orientations at the initial position and when the manipulator had entered the cabin along the known path are shown in Fig. 16a and Fig. 16b, respectively. At this time, the end effector and the 11th and 12th universal joints had entered the cabin.   Fig. 16c shows the orientation when the end effector had reached the end point, C. The end effector and the 11 th and 12 th universal joints were located in one cabin, and the 9 th and 10 th universal joints were located in another cabin. Fig. 16d shows that the end effector returned along the known path. Fig. 16e shows the end effector and the universal joints when the hyper-redundant manipulator had completely exited the cabins. These inverse solutions are shown in Fig. 17. Simulation experiments based on these inverse solutions verified the correctness and effectiveness of the new algorithm. Using the homogeneous coordinate transformation method to derive the positive kinematics equation for a hyper-redundant manipulator can reduce the number of coordinate systems, simplify the derivation process for the transformation matrix, and save calculation time. An improved modal backbone curve method was proposed in this study. First, with changes in the discrete points along the known path, the backbone curves that the hyper-redundant manipulator used to reach these points were dynamically obtained. Then, the joints of the hyper-redundant manipulator were fitted to the modal backbone curves. Finally, the inverse kinematics of the hyper-redundant manipulator were solved based on the spatial geometry method. This method solved the motion-planning problem of an industrial hyper-redundant manipulator entering a known narrow environment.
Engineering application experiments verified the hyper-redundant manipulator's ability to move through curved pipes and narrow workspace areas. The effectiveness of the new algorithm was also proven by these experiments.

Conflicts of Interest:
The authors declare that they have no conflicts of interest to report regarding the present study.