An explicit solution of forward and inverse kinematics for a serial manipulator with insufficient intersection joints based on finite and instantaneous screw theory

Abstract

To satisfy the requirements of large workspace, high capability, and low inertia demanded by the high precision and high stiffness aerospace equipment, serial manipulator with insufficient intersection joints is proposed in this article based on finite and instantaneous screw theory. Due to the specific topology structure with no intersecting joints at both ends, the explicit solution of forward and inverse kinematics for the serial manipulator with insufficient intersection joints proposed in this article is extremely difficult. Therefore, an analytical method based on finite and instantaneous screw theory is proposed for this serial manipulator, and the explicit kinematic solution is derived in this article. According to particular calculation principle of finite and instantaneous screw theory, the finite motion of this serial manipulator is described. Based on this description, the product of exponentials formula of the kinematics of the serial manipulator with insufficient intersection joints is established on the basic of finite and instantaneous screw theory for the first time, and a novel subproblem is arose during the decomposition of product of exponentials formula. Regarding this issue, an innovative approach to this subproblem is proposed by introducing the new Paden–Kahan subproblem with elimination, and the explicit solution for the inverse kinematics is derived. Finally, the numerical examples are simulated, and the correctness of the method proposed in this article is verified.

Introduction

With the acceleration of space exploration by countries around the world, aerospace equipment is constantly being updated. As one of the most important equipment supporting space science, the space manipulator mainly provides orbital services such as target capture, racemization, rotation, and insertion. ^{1 –4} At present, the space manipulators used on space stations in various countries are composed of serial mechanisms or multiple serial mechanisms, which have the advantages of large workspace, high flexibility, and high attitude. However, as space exploration continues to deepen and space operations become more complex, serial mechanisms will face enormous challenges in meeting the demand for high precision and high stiffness tasks.

In the light of this problem, Song et al. ⁵ proposed a hybrid mechanism configuration by combining a serial mechanism with a parallel mechanism of high stiffness and precision, where the parallel mechanism is installed at the end of the serial mechanism. The configuration with the advantages of large workspace, high flexibility, high stiffness, and high precision was provisionally achieved, and an integrated analysis of the parallel mechanism was conducted. Based on the hybrid mechanism of the above model, Zhang and Qi ⁶ synthesized the configurations of the serial mechanism and the parallel mechanism, respectively, and topologically developed various novel configurations that allow further optimization based on performance requirements. Therefore, it is of great significance to carry out relevant research on this type of hybrid mechanism as a new field of innovation and development for mechanism science in space manipulators.

Due to the great advantages of high precision, high stiffness, and large workspace combined with the hybrid manipulator, it has developed very rapidly. ^2,7,8 The hybrid mechanisms could be classified into three categories according to their structural character: The first type is using the parallel mechanism as a base to connect the serial mechanism. The second type is a combination of several parallel mechanisms. The third type is using the serial mechanism as a base to connect the parallel mechanism. The first type of hybrid mechanism is represented by the Tricept robot, which has high precision, high stiffness, and a large workspace, and is widely used for high-speed machining. ^{9 –11} The second type of hybrid mechanism is limited by the complexity of the structure, and most research is still at the theoretical or experimental stage. ^{12 –15} The third type of hybrid mechanism has limited research due to the high-end load requirements of the serial manipulator and the high degree of implementation difficulty.

Although the first type of hybrid mechanism is relatively mature, it still does not have the same flexibility and workspace as a serial manipulator of the same size. Besides, the cramped working environment inside the space station is not suitable for the use of such a hybrid manipulator. The second type of hybrid mechanism significantly improves its workspace and flexibility compared to the first, but this complex system is also more affected by various uncertainties in space. The third type of hybrid mechanism perfectly inherits the advantages of both the serial mechanism and the parallel mechanism. The space manipulator is almost unaffected by gravity in the space station environment, which means that the installed parallel mechanism has a relatively minimal effect on the end of the serial manipulator, making it suitable for use as a space manipulator. During operation, the serial manipulator performs a wide range of motions, while the parallel mechanism compensates for precise positioning.

Thus, the research team obtained a variety of novel topological configurations by the configuration synthesis method and proposed a five-degree of freedom (DOF) topological configuration of the serial mechanism with insufficient intersection joints. This configuration has a relatively low inertia while maintaining dexterity. The multiple joints of the manipulator are offset to increase its foldability. The single joints were designed on both sides of the manipulator to reduce the motion coupling between the parallel mechanism and serial mechanisms, thus reducing the difficulty of analyzing the hybrid mechanism. However, due to its peculiar structure, it is little studied and no relevant kinematic modelling is available, so the kinematic analysis of this mechanism is taken as the research object in this article.

The forward kinematics of serial manipulators is relatively simple, and the D-H method and the screw method are commonly used. ¹⁶ Whereas the inverse kinematics of serial manipulators is more complex, the solution methods can be divided into two categories: the numerical method and the algebraic method. The numerical method is suitable for almost all manipulators, but the calculation results are influenced by the initial values. ^{17 –19} The analytical method is more numerically stable than the former, including the geometric method and the algebraic methods, both of which depend on the structural characteristics of the manipulator in terms of the difficulty of the solution. ²⁰

Paden–Kahan is a geometric approach to the inverse kinematics of a manipulator that is efficient and ready to model but is limited by the manipulator not having enough intersecting joints. The earlier studies ^{21 –24} created novel Paden–Kahan subproblems according to the nonintersecting joints of the specific manipulator in obtaining the inverse solution and provided the solution process. Chen and Gao ²⁵ proposed an equivalent motion technique, which transformed the kinematic equation of the manipulator into the standard form by replacing the sequential of the kinematic pair, and solved it with classical Paden–Kahan subproblems. Wang et al. ²⁶ proposed a method that combines elimination to reduce the dependence of simple subproblems on intersecting joints.

Modern mechanism research has found that the continuous motion of any rigid body is essentially Chasles’ motion, and the finite and instantaneous screw (FIS) theory contains the basic elements of Chasles’ motion, which reveals the simplest characterization of the continuous motion of a mechanism, and provides an integrated method for studying the mechanism. ^{27 –30} In addition, this theory rigorously derives the differential mapping relationship between continuous and instantaneous motions, which allows configuration synthesis, kinematics, dynamics, and parameter optimization to be carried out under a unified mathematical framework, which is conducive to the integrated analysis of the mechanism. ³¹

Therefore, in this article, the kinematics of a serial manipulator with insufficient intersecting joints is carried out based on FIS theory. The product of exponentials (POE) formula of the serial manipulator with insufficient intersecting joints is established analytically and explicitly for the first time based on FIS theory. Regarding the lack of explicit inverse kinematics solution for this manipulator without intersecting joint at both ends, an innovative approach is proposed by introducing the new Paden–Kahan subproblem with elimination, and the explicit inverse kinematics is derived based on FIS theory.

Finite screw and forward kinematics model

The kinematics of the manipulator mainly analyzes the end pose relative to the initial pose of the manipulator from the perspective of mechanism. First, considering the position of a rigid body in space, defining {S} as the inertial coordinate system at the initial position and {T} as the coordinate system fixed to the rigid body, the orientation of the rigid body is represented by the rotation matrix R of {T} relative to {S}. Then, the position of the rigid body can be represented by the vector p of {T} relative to {S}, as shown in Figure. 1.

Figure 1.

Pose of rigid body in space.

Chasles and Poinsot proved that any motion of a rigid body is equivalent to movement along and rotation around a certain line in the space, which is commonly called screw motion. With the development of modern mechanism kinematics, this motion can be expressed clearly and intuitively by FIS theory. ³² For continuous of any rigid body, where

$S_f=2\text{tan}\frac{\theta }{2}\left(\begin{array}{c}{s}_f\\ r_f\times s_f\end{array}\right)+t\left(\begin{array}{c}\mathbf{0}\\ s_f\end{array}\right)$

1

where $s_f$ is the unit direction vector of the axis, $r_f$ is any point on $s_f$ , and the $\theta$ and t are the rotation angle and translation distance about/along the axis, respectively. As shown in Figure. 2, when $\theta \ne 0$ , it represents a finite screw with a pitch of $\theta /t$ . When $\theta =0$ , it represents pure translation with infinite pitch.

Figure 2.

Finite screw.

For a serial manipulator composed of n joints, there are n $s_f$ s. According to the screw triangle theorem as

$\begin{array}{l}{S}_{f,12}=S_{f,1}\Delta S_{f,2}=\frac{S_{f,1}+S_{f,2}+\frac{S_{f,2}\times S_{f,1}}{2}-\text{tan}\frac{{\theta }_1}{2}\text{tan}\frac{{\theta }_2}{2}\left(t_2\left(\begin{array}{c}\mathbf{0}\\ s_{f,1}\end{array}\right)+t_1\left(\begin{array}{c}\mathbf{0}\\ s_{f,2}\end{array}\right)\right)}{1-\text{tan}\frac{{\theta }_1}{2}\text{tan}\frac{{\theta }_2}{2}{s}_{f,1}^{\text{T}}{s}_{f,2}}\end{array}$

2

where “ $\Delta$ ” denotes the screw triangle product.

$S_{f,2}\times S_{f,1}=\left(\begin{array}{c}2\text{tan}\frac{{\theta }_2}{2}{s}_{f,2}\times 2\text{tan}\frac{{\theta }_1}{2}{s}_{f,1}\\ \begin{array}{l}2\text{tan}\frac{{\theta }_2}{2}{s}_{f,2}\times \left(2\text{tan}\frac{{\theta }_1}{2}\left(r_{f,1}\times s_{f,1}\right)+t_1{s}_{f,1}\right)+\\ \left(2\text{tan}\frac{{\theta }_2}{2}\left(r_{f,2}\times s_{f,2}\right)+t_1{s}_{f,2}\right)\times 2\text{tan}\frac{{\theta }_1}{2}{s}_{f,1}\end{array}\end{array}\right)$

3

The finite screw of the rigid body is extended to the forward kinematics of the manipulator with n DOFs. Defining $g_{st}(0)$ / $g_{st}(\theta )$ as homogeneous transformations between the {S} and {T} of the initial/target positions of the manipulator. The POE formula of the serial manipulator based on the FIS theory as

$g_{st}(\theta )=S_{f,1}\Delta S_{f,2}\Delta S_{f,2}\cdots \Delta S_{f,n}{g}_{st}(0)$

4

Forward kinematics of the manipulator

To obtain the forward kinematics equation of the manipulator, selecting the axis of each joint, the initial and end position of {S} and {T} as shown in Figure 3.

Figure 3.

Twist coordinate system of the manipulator.

The angles of each joint are 0, and the initial pose of the manipulator is

$g_{st}(0)=\left[\begin{array}{cccc}1& 0& 0& a_6\\ 0& 1& 0& l_1\\ 0& 0& 1& -a_5\\ 0& 0& 0& 1\end{array}\right]$

5

Establishing the twist on each joint relative the inertial coordinate system as

$\begin{array}{ccccc}{s}_{f,1}=\left[\begin{array}{l}0\\ 0\\ 1\end{array}\right]& s_{f,2}=\left[\begin{array}{l}0\\ 0\\ 1\end{array}\right]& s_{f,3}=\left[\begin{array}{l}0\\ 1\\ 0\end{array}\right]& s_{f,4}=\left[\begin{array}{l}0\\ 1\\ 0\end{array}\right]& s_{f,5}=\left[\begin{array}{l}0\\ 0\\ 1\end{array}\right]\end{array}$

6

Selecting a point on the axis of each joint as

$\begin{array}{cccc}{r}_{f,1}=\left[\begin{array}{l}0\\ 0\\ 0\end{array}\right]& r_{f,2}=r_{f,3}=\left[\begin{array}{c}0\\ a_1\\ -a_2\end{array}\right]& r_{f,4}=\left[\begin{array}{l}0\\ 0\\ 0\end{array}\right]& r_{f,5}=\left[\begin{array}{c}{a}_6\\ l_1\\ 0\end{array}\right]\end{array}$

7

From the expression of the finite screw based on FIS theory, the unit twist on each joint be obtained as

$\begin{array}{ccccc}{\xi }_1=\left[\begin{array}{c}0\\ 0\\ 1\\ 0\\ 0\\ 0\end{array}\right]& {\xi }_2=\left[\begin{array}{c}0\\ 0\\ 1\\ a_1\\ 0\\ 0\end{array}\right]& {\xi }_3=\left[\begin{array}{c}0\\ 1\\ 0\\ 0\\ 0\\ 0\end{array}\right]& {\xi }_4=\left[\begin{array}{c}0\\ 1\\ 0\\ 0\\ 0\\ 0\end{array}\right]& {\xi }_5=\left[\begin{array}{c}0\\ 0\\ 1\\ l_1\\ -a_6\\ 0\end{array}\right]\end{array}$

8

According to equation (4), the forward kinematics model is established

$\begin{array}{l}{g}_{st}(\theta )=S_{f,1}{S}_{f,2}{S}_{f,3}{S}_{f,4}{S}_{f,5}{g}_{st}(0)=g_{st}(\theta )S_{f,12345}{g}_{st}(0)\end{array}$

9

Inverse kinematics analysis

Inverse kinematics based on Paden–Kahan subproblems

The kinematic model of the manipulator based on the POE formula can be divided into several Paden–Kahan subproblems to solve the inverse problem by simplifying the kinematic model. These subproblems are actually some geometric problems with clear meaning, and Paden and Kahan have proposed three subproblems and provided solutions.

(1) Subproblem 1: rotate around a fixed axis.

As shown in Figure. 4, the point p in space rotates θ around a fixed twist ${\xi }_1$ to point q, solving the θ. Define vector $u=p-r$ , $v=q-r$ , $u^{\prime }$ , $v^{\prime }$ are projections of u, v perpendicular to the twist ${\xi }_1$ plane. The solution of Paden–Kahan subproblem 1 as

$\theta =\text{atan}2({\omega }^T(u^{\prime }\times v^{\prime }),u^{\prime }\cdot v^{\prime })$

10

Figure 4.

Paden–Kahan subproblem 1.

• (2) Subproblem 2: rotate around two intersecting fixed axes sequentially.

As shown in Figure 5, there are two intersecting fixed twists ${\xi }_1$ and ${\xi }_2$ in space. The point p in space rotates ${\theta }_2$ around the twist ${\xi }_2$ to point c and then rotates ${\theta }_1$ around the ${\xi }_1$ to point q, solving ${\theta }_1$ and ${\theta }_2$ . If twist ${\xi }_1$ and twist ${\xi }_2$ are coincident, the subproblem 2 degenerates to subproblem 1, and any ${\theta }_1$ and ${\theta }_2$ satisfying ${\theta }_1+{\theta }_2=\theta$ are solutions. While twist ${\xi }_1$ and twist ${\xi }_2$ are not coincident, c is the intersection point of two finite screws, which satisfies $S_{f,2}p=c=S_{f,1}q$ . Defining vector $u=p-r$ , $v=q-r$ , $z=c-r$ , calculating point c from equations (11) to (14), then solve according to two Paden–Kahan subproblems 1

$z=\alpha {\omega }_1+\beta {\omega }_2+\gamma ({\omega }_1\times {\omega }_2)$

11

$\alpha =\frac{({\omega }_1^{\text{T}}{\omega }_2){\omega }_2^{\text{T}}v-{\omega }_1^{\text{T}}u}{{({\omega }_1^{\text{T}}{\omega }_2)}^2-1}$

12

$\beta =\frac{({\omega }_1^{\text{T}}{\omega }_2){\omega }_1^{\text{T}}v-{\omega }_2^{\text{T}}u}{{({\omega }_1^{\text{T}}{\omega }_2)}^2-1}$

13

$\gamma =\frac{{\Vert u\Vert }^2-{\alpha }^2-{\beta }^2-2\alpha \beta {\omega }_1^{\text{T}}{\omega }_2}{{\Vert {\omega }_1\times {\omega }_2\Vert }^2}$

14

Figure 5.

Paden–Kahan subproblem 2.

• (1) Subproblem 3: rotate around a fixed axis until it is a specified distance from a point.

As shown in Figure 6, the point p rotates around the twist ${\xi }_1$ until the distance from another point q in space is $\delta$ , solving the θ. Defining vector $u=p-r$ , $v=q-r$ , the solution of Paden–Kahan subproblem 3 as

$\theta ={\theta }_0\pm \text{arccos}\left(\frac{{\Vert u^{\prime }\Vert }^2+{\Vert v^{\prime }\Vert }^2-{\delta }^2}{2\Vert u^{\prime }\Vert \Vert v^{\prime }\Vert }\right)$

15

${\theta }_0=\text{atan}2({\omega }^T(u^{\prime }\times v^{\prime }),u^{\prime }\cdot v^{\prime })$

16

Figure 6.

Paden–Kahan subproblem 3.

Standard industrial serial manipulators have a wrist joint composed of three intersecting rotating joints. Models as shown in Figures 7 and 8, the wrist joint of two common industrial manipulators is designed at its end position and initial position, respectively, which can facilitate the adjustment of orientation in space. The other three joints cooperate with each other to allow the end-effector to reach any pose in space.

Figure 7.

Manipulator model with anterior wrist joint.

Figure 8.

Manipulator model with posterior wrist joint.

To solve the inverse kinematics problem for these manipulators, it is usually necessary to first simplify the twists on all intersecting joints in the POE formula according to the principle of position invariance. Then the twist of the first set of simplified intersecting joints is eliminated according to the principle of distance invariance. The POE formula is divided into several Paden–Kahan subproblems, and the inverse kinematics of the manipulator can be expediently obtained by substituting equations (10) to (16), the modeling process is shown in Figure 9.

Figure 9.

Inverse kinematics modeling process of manipulator.

The kernel problem of the manipulator with insufficient intersection joints

The classic Paden–Kahan subproblems be used to solve the manipulator with enough intersecting joints at initial and end position. In the manipulator studied in this article, joint 1, joint 2 and joint 5 are parallel, while axis 3 and axis 4 are parallel, and there are no intersecting joints at either end. The following problems arise during the simplification process:

Multiple twists cannot be simplified simultaneously through the principle of position invariance.

Twists cannot be eliminated sufficiently through the principle of distance invariance.

The two points above make it impossible to convert the POE formula into classical Paden–Kahan subproblems. Therefore, elimination combined with screw is proposed to obtain the inverse solution according to the characteristics of this manipulator. In equation (9), the twist ${\xi }_5$ is uncoupled to the finite screw of others nor does it change the position of end point. Therefore, joint 5 can be solved by elimination or Euler angles. The POE formula is simplified according to the principle of position invariance. The twist ${\xi }_4$ and twist ${\xi }_1$ can be replaced by any known point on the axis of the respective joint. The other joint 2 intersects with joint 3 to form the wrist joint with 2-DOF in the middle position of the manipulator. Applying the principle of distance invariance to eliminate the twists ${\xi }_1$ , the problem is transformed into rotating around two intersecting and noncoincident twists in turn until the distance to a point in space is $\delta$ . Therefore, if the position after the rotation is calculated, then the rotation angle problem is solved in accordance with subproblem 2. As shown in Figure 10, selecting two points q ₁ and q ₂ in space, and point p rotates ${\theta }_2$ , ${\theta }_1$ around the interesting twists ${\xi }_2$ and ${\xi }_1$ , respectively, to reach point q. The distance between point q and point q ₁/q ₂ is ${\delta }_1$ / ${\delta }_2$ . If the point q satisfies the above conditions, it is located at the intersection of three spherical surfaces with centers q ₁, q ₂, r, and radius ${\delta }_1$ , ${\delta }_2$ , $\Vert p-r\Vert$ . ³³

Figure 10.

Paden–Kahan subproblem 2 expansion.

Defining vector $z=q-r$ , defining the unit vector ${\omega }_1=(q_1-r)/\Vert q_1-r\Vert$ , ${\omega }_2=(q_2-r)/\Vert q_2-r\Vert$ and ${\omega }_1\times {\omega }_2$ . The three vectors are linearly independent, so that z can be represented linearly as

$z=\alpha {\omega }_1+\beta {\omega }_2+\gamma ({\omega }_1\times {\omega }_2)$

17

Squaring the norm on both sides of equation (17)

${\Vert z\Vert }^2={\alpha }^2+{\beta }^2+2\alpha \beta {\omega }_1^{\text{T}}{\omega }_2+{\gamma }^2{\Vert {\omega }_1\times {\omega }_2\Vert }^2$

18

Based on the geometric relationship of the intersection of spheres, the parameters in equation (16) can be obtained, and point q can be solved by combining equations (19) to (21)

$\alpha =\frac{{\text{t}}_1-({\omega }_1^{\text{T}}{\omega }_2){\text{t}}_2}{1-{({\omega }_1^{\text{T}}{\omega }_2)}^2}$

19

$\beta =\frac{{\text{t}}_2-({\omega }_2^{\text{T}}{\omega }_1){\text{t}}_1}{1-{({\omega }_1^{\text{T}}{\omega }_2)}^2}$

20

$\gamma =\frac{{\Vert z\Vert }^2-{\alpha }^2-{\beta }^2-2\alpha \beta {\omega }_1^{\text{T}}{\omega }_2}{{\Vert {\omega }_1\times {\omega }_2\Vert }^2}$

21

Inverse kinematics of the manipulator

Solutions of Θ₅, Θ₂, and Θ₃

In equation (9), ${\theta }_5$ can be obtained by using inverse trigonometric function as

$n_z=-{\mathrm{c}}_5{s}_{34}$

22

$n_z=s_5{s}_{34}$

23

${\theta }_5=\text{atan}2(-\frac{s_5{s}_{34}}{-c_5{s}_{34}})$

24

Moving the known quantity to the right of equation (9), and the equation on the left only contains the unknown quantity as

$\begin{array}{l}{g}_{st}(\theta )=S_{f,1}\Delta S_{f,2}\Delta S_{f,3}\Delta S_{f,4}\Delta S_{f,5}\\ \begin{array}{ccc}& & =g_{st}{(\theta )}^{-1}{g}_{st}(0)=g(1)\end{array}\end{array}$

25

$S_{f,1}\Delta S_{f,2}\Delta S_{f,3}\Delta S_{f,4}=g(1)S^{-1}{}_{f,5}=g(2)$

26

Selecting a point in the direction of the joint axis, the position of a point on the axis of rotation around the axis remains unchanged as

$S_{f,1}\Delta S_{f,2}\Delta S_{f,3}p=g(2)p=g(3)$

27

Selecting $q_1=[00q_{1z}]$ and $q_1=[00-q_{1z}]$ as the two points on the joint axis of twist ${\xi }_1$ , a point outside the axis of rotation rotates around the axis, and the distance to a given point on the axis remains unchanged as

$S_{f,1}\Delta S_{f,2}\Delta S_{f,3}p-q_1=g(3)-q_1$

28

$S_{f,1}\Delta S_{f,2}\Delta S_{f,3}p-q_2=g(3)-q_2$

29

$\Vert S_{f,1}\Delta S_{f,2}\Delta (S_{f,3}p-q_1)\Vert =\Vert S_{f,2}\Delta S_{f,3}p-q_1\Vert =\Vert g(3)-q_1\Vert$

30

$\Vert S_{f,1}\Delta S_{f,2}\Delta (S_{f,3}p-q_2)\Vert =\Vert S_{f,2}\Delta S_{f,3}p-q_2\Vert =\Vert g(3)-q_2\Vert$

31

Equations (30) and (31) correspond to the form of Paden–Kahan subproblem 2 expansion, which involves rotating around two intersecting and noncoincidence twists in sequence until the distance from point q in space is $\delta$ , solve point p by equations (17) to (21), and convert this problem into subproblem 2

$S_{f,2}\Delta S_{f,3}p=q$

32

Solving the intersection point $c=[c_x{c}_y{c}_z]$ of the midpoint p of subproblem 2 about the finite screw of two twists by equations (11) to (14)

$S_{f,1}p=c=S_{f,2}q$

33

Equation (33) is equivalent to two Paden–Kahan subproblems 1, solving ${\theta }_2$ and ${\theta }_3$ by equation (10)

$S_{f,2}p=c$

34

$S^{-1}{}_{f,3}c=q$

35

$\begin{array}{c}{\theta }_2=\text{atan}2\left({\omega }_2^{\text{T}}\right(u_2^{\text{'}}\times v_2^{\text{'}}),u_2^{\text{'}}\cdot v_2^{\text{'}})\\ \text{=arctan}\frac{(c_y-r_y)(p_x-r_x)-(c_x-r_x)(p_y-r_y)}{(c_x-r_x)(p_x-r_x)+(c_y-r_y)(p_y-r_y)}\end{array}$

36

$\begin{array}{l}{\theta }_3=\text{atan}2\left({\omega }_3^{\text{T}}\right(u_3^{\text{'}}\times v_3^{\text{'}}),u_3^{\text{'}}\cdot v_3^{\text{'}})\\ \text{=arctan}\frac{(c_z-r_z)(q_x-r_x)-(c_x-r_x)(q_z-r_z)}{(c_x-r_x)(q_x-r_x)+(c_z-r_z)(q_z-r_z)}\end{array}$

37

Solutions of Θ₁

Substituting known quantities ${\theta }_2$ and ${\theta }_3$ into equation (27), the position of a point on the axis of rotation around the axis remains unchanged as

$S_{f,1}\Delta (S_{f,2}\Delta S_{f,3}p)=S_{f,1}g=g(3)$

38

Equation (38) corresponds to the form of Paden–Kahan subproblem 1, solving ${\theta }_1$ by equation (10)

$\begin{array}{l}{\theta }_1=\text{atan}2\left({\omega }_1^{\text{T}}\right(u_1^{\text{'}}\times v_1^{\text{'}}),u_1^{\text{'}}\cdot v_1^{\text{'}})\\ \text{=arctan}\frac{g_3{}_y{g}_x-g_{3x}{g}_y}{g_{3x}{g}_x+g_3{}_y{g}_y}\end{array}$

39

Solutions of Θ₄

Moving the known quantity ${\theta }_1$ , ${\theta }_2$ , ${\theta }_3$ to the right of equation (26), selecting a point $f=[111]$ that is not on the twist ${\xi }_4$ , the position of a point on the axis of rotation around the axis remains unchanged as

$S_{f,4}f=S^{-1}{}_{f,4}\Delta S^{-1}{}_{f,3}{S}^{-1}{}_{f,2}\Delta S^{-1}{}_{f,1}g(2)f=g(4)$

40

Equation (40) corresponds to the form of Paden–Kahan subproblem 1. Solve ${\theta }_4$ by equation (10)

$\begin{array}{l}{\theta }_4=\text{atan}2\left({\omega }_4^{\text{T}}\right(u_4^{\text{'}}\times v_4^{\text{'}}),u_4^{\text{'}}\cdot v_4^{\text{'}})\\ \text{=arctan}\frac{(g_{4z}-p_z)(p_x-1)-(g_{4x}-p_x)(p_z-1)}{-(g_{4x}-p_x)(p_x-1)-(g_{4z}-p_z)(p_z-1)}\end{array}$

41

Simulation and verification

Simulation and verification of forward kinematics

The parameters of each link of the manipulator are as

$\begin{array}{l}{a}_1=260.00mm\; a_2=130.00mm\; a_3=285.00mm\\ a_4=197.50mm\; a_5=201.50mm\; a_6=71.00\hspace{0.17em}\text{mm}\end{array}$

The forward kinematics formula of the manipulator is written into MATLAB, giving a set of joint angles and programming the trajectory function, plotting the Cartesian trajectory of the tool coordinate system relative to the inertial coordinate system. Enabling the motion plug-in in SolidWorks and adding motors to the 3D model and input the same set of joint angles. Adding a diagram of the trajectory of the end position of the manipulator in space and importing the data into MATLAB. The process is shown in Figure 11.

Figure 11.

Forward kinematics simulation process.

In the initial position of the manipulator, all joint angles are set to 0. Given a set of joint angles as the motion trajectory of the manipulator to validate the POE model as equation (42)

${\theta }_1={\theta }_2={\theta }_3={\theta }_4={\theta }_5=\pi /2$

42

As shown in Figure 12, the forward kinematics model established by the POE formula based on the FIS theory matches the trajectory of the 3D model in SolidWorks in three directions in space, which verify the correctness of kinematics model.

Figure 12.

End-effector position in space.

As shown in Figure 13, the error of the position is negligible, which confirms the accuracy and efficiency of this forward kinematic model.

Figure 13.

The error of position.

Simulation and verification of inverse kinematics

Paden–Kahan subproblems are to solve the inverse solution of manipulators based on geometric methods which are the closed form solution of manipulators. According to subproblem 2, the value of q can be two sets, one set, and no solution. Ideally, each group of ${\theta }_2$ and ${\theta }_3$ corresponds to ${\theta }_1$ and ${\theta }_4$ , and there are at most four sets of solutions. But in reality, the manipulator cannot produce a full 6-DOF motion. The wrist joint with 2-DOF in the middle position of the manipulator may not satisfy the position in the space; as a result, some positions in space not satisfy multiple solutions.

To verify the correctness and accuracy of the inverse solution, joint and Cartesian trajectory two method are used for simulation.

(1) Joint trajectory: To simplify calculations, assume that each joint maintains a constant velocity, the joint trajectory as

$m:q_i={[1.8,-1.8,0.9,-0.9,1.2]}^{\text{T}}\ast t,t=0,1,2\dots 50$

43

In equation (43), i represents each joint. Substituting m into equation (9) to calculate the trajectory of the manipulator in Cartesian space, and then calculating the sequence of joint angles through inverse kinematics algorithm proposed in this article. Comparing the calculated results with the initialized joint trajectory m, as shown in Figure 14.

Figure 14.

The joint trajectory of the calculation and m.

As shown in Figure 15, the error of each joint is calibrated.

Figure 15.

The error of each joint.

• (2) Cartesian trajectory: Given a Cartesian trajectory as

$n:\{\begin{array}{l}x=400\\ y=50\ast (2\ast \text{cos}(2\pi t)-\text{cos}(2\pi t))\\ z=50\ast (2\ast \text{sin}(2\pi t)-\text{sin}(2\pi t))\end{array}$

44

Similarly, assume that each joint maintains a constant velocity, $t=0,0.05,0.10\dots 1.$ Then calculating the sequence of joint angles as shown in Figure 16.

Figure 16.

The sequence of joints angle.

Substituting the calculation into equation (9) to obtain the trajectory of the manipulator, comparing the calculated results with the initialized Cartesian trajectory n, as shown in Figure 17.

Figure 17.

The Cartesian trajectory of the calculation and n.

As shown in Figure 18, the error of each position is calibrated.

Figure 18.

The error of the position.

Based on the simulation results above, the error of joints and position is very small and can be ignored, which indicates that the inverse kinematic algorithm proposed in this article has high accuracy.

Conclusion

To meet the requirements of on-orbit missions for large workspace, high stiffness, and high precision, the kinematics of a serial manipulator without insufficient intersection joints is analyzed based on the FIS theory in this article. The main contributions are as follows:

Based on the FIS theory, the continuous motion of the manipulator is described, and the POE formula for a serial manipulator is established for the first time by using the triangular product algorithm of finite screw.

In the process of establishing the inverse solution model of the manipulator, an innovative method combining elimination with the expansion of Paden–Kahan subproblem 2 is proposed to solve the new subproblem caused by the structural characteristics of the manipulator without intersection joint on both sides, and the explicit solution is given.

The correctness of the kinematic model based on FIS theory is verified through simulation, and the end error of the manipulator is verified to be within a reasonable range by two numerical examples.