# Kinematical Research of Free-Floating Space-Robot System at Position Level Based on Screw Theory

Authors: Yong Wang; Xiaojun Liang; Kejie Gong; Ying Liao
Journal: International Journal of Aerospace Engineering
 (2019)

---

## Abstract

Kinematics of a free-floating space-robot system is a fundamental and complex subject. Problems at the position level, however, are not considered sufficiently because of the nonholonomic property of the system. Current methods cannot handle these problems simply and efficiently. A novel and systematical modeling approach is provided; forward and inverse kinematics at the position level are deduced based on the product of exponentials (POE) formula and conservation of linear momentum. The whole deduction process is concise and clear. More importantly, inertial tensor parameters are not introduced. Then, three situations with different known variables are mainly studied. Due to the complexity of inverse kinematical equations, a numerical method is proposed based on Newton’s iteration method. Two calculation examples are given, a dual-arm planar model and a single-arm spatial model; both forward and inverse kinematical solutions are given, while inverse kinematical results are compared with simulation results of Adams. The results indicate that the proposed methods are quite accurate and efficient.


---

## Body

## 1. Introduction

Kinematics is a basic subject of robotics. It includes forward and inverse kinematics at the position and the velocity (differential) level. Mature traditional means have been applied to the research of ground-based robots, such as the Denavit–Hartenberg method [1]. Kinematics of space robots (also referred to as space manipulators) is more complicated due to the base-arm dynamic coupling. For example, the base is completely free in reaction to the arms’ movement when the system is under a free-floating situation [2], which is one of the key researches due to its low fuel consumption. And, motion of the base disturbs the pose (positions and orientations) of the end-effector inversely.New theories were developed for the kinematics of a space-robot system. Vafa and Dubowsky [3] proposed an ingenious modeling method called the Virtual Manipulator (VM) method. It transforms the real model into a series of virtual-link vectors connected to a fixed base called the Virtual Ground (VG), by using conservation of linear momentum. VG is just located at the centroid of the whole system and is motionless in inertial space. Based on the VM method, Xu et al. [4] presented a concept of “system centroid equivalent manipulator”, whose kinematic model is derived. Then, the trajectory of the balance arm for stabilizing the base centroid is planned. Umetani and Yoshida [5] developed another modeling idea, which defines the generalized Jacobian matrix (GJM). GJM is derived by using the momentum conservation and geometrical relations; it contains inertial parameters of the system. This method is largely used to build the dynamic model of space robots [6–8]. Liang et al. [9] presented another concept called the Dynamically Equivalent Manipulator (DEM). DEM mapped a space manipulator into a fixed-base manipulator which preserves both its dynamic and kinematic properties. It can be physically built for experimental study. Moosavian and Papadopoulos [10] developed and compared two approaches for the kinematics modeling of multiplying robots, called the barycentric vector approach and the direct path method, and concluded that the latter requires significantly less computations. However, inverse kinematics is not discussed.As angular momentum equations cannot be integrated [11], the space-robot system is nonholonomic. Therefore, the inverse kinematics at the position level cannot be solved only by the pose of the end-effector. This defect leads to that current studies mainly focused on differential kinematics. They usually have to do integration, however, when just joint displacements at the position level are needed in some scenarios, such as path planning [12, 13]. The above-mentioned methods could not solve the kinematics at the position level simply and efficiently. The VM and DEM methods focus on transformation of the model. After transformation, the system is just the same as the ground-based one so that traditional means can be applied. Nevertheless, the transformation process significantly increases the workload, which will be heavier if the system is more complex. The GJM method is derived by using body-fixed vectors, which does not make it easy to program. And its equation is in differential form, which means it cannot be applied to solve the kinematics at the position level directly. What is more, the equations of GJM and DEM contain dynamic parameters, which are not necessary to solve kinematical problems at the position level. Liang and Xu [14] discussed the possibility of an inverse kinematical solution at the position level when there are some constraints and known variables. Joint angles of a single-arm space robot are calculated by using the D-H method. A multiarm space-robot system is not concerned. To this end, this paper focused on the kinematics of a free-floating space-robot system at the position level, aimed at developing a simple and efficient method. Moreover, the method should cover forward and inverse kinematics and single-arm and multiarm space-robot systems.Screw theory is an efficient alternative to analysis kinematics of a rigid body and mechanism. It was first systematized by R.S. Ball, then employed and improved for study on ground-based manipulators [15, 16] and parallel robots [17, 18]. A screw depicts a dual vector, so it can represent both translation and rotation, which gives screw theory the advantages of unity and simplicity. As a basic and conventional tool in screw theory to study robotic kinematics, the product of exponentials (POE) formula is derived by the successive screw displacement of a kinematic chain. Rocha et al. [19] compared the screw-based method with the D-H method in detail and concluded that the former has advantages over the D-H one, although the D-H method is more commonly used. Nevertheless, few studies on a space-robot system using screw theory are found in current research. Liu and Wu [20] deduced the dynamic model of a space-robot system based on the screw theory and Kane equation. Liu and Wu [21] provided a simple and efficient method to deduce the Jacobian matrix of a free-floating branching robot system, by dividing it into two parts, a ground robot with a fixed base and a locking joint robot with a floating base. Kinematics at the position level is not considered.Taking the advantages of the screw theory, this paper provides a novel and systematical approach. Forward and inverse kinematical equations of a free-floating space-robot system at the position level are established based on the POE formula and conservation of linear momentum, without employing inertial tensors. And several situations with different known variables are discussed: only position of the base is known, only orientation of the base is known, and both the position and orientation of the base are measurable. A numerical means based on Newton’s iteration method is given, considering that the closed-form inverse solution may be hard to calculate directly when the system is complex. The entire approach can be applied to space robots at free-floating mode without regard to the number of arms.


## 2. The Model and Basic Theories

### 2.1. The Model of the Space-Robot System

A space-robot system consists of the base (satellite) and arms (also called manipulators, single or multiple) typically. Each arm consists of several links, joints, and an end-effector (EE). The following assumptions are made to build a concise and accurate model.(1)
The position and orientation of the base is not controlled, and there are no external forces on the system, which means the system is at free-floating mode
(2)
Each arm is a single chain connected to the base, and the parallel manipulator is not considered
(3)
All links and joints are rigid
(4)
All joints are revolute, and all the rotation angles are limited between−180∘ and 180∘
(5)
The number of dofs of each arm is no greater than six. For a planar model, it is no greater than three. A redundant system is not consideredA model is provided in Figure1. The definitions of each symbol are given as follows.
(i)
∑I represents the inertial frame and is located at the centroid of the whole system
(ii)
∑B represents the base-fixed frame
(iii)
VectorpI0 represents the centroidal position of the base in the inertial frame
(iv)
VectorpIik represents the centroidal position of the kth arm’s ith link in the inertial frame
(v)
VectorpIEk represents the position of the kth arm’s EE in the inertial frameFigure 1
The model of a space-robot system. The system hasN arms: only the 1th, kth, and Nth arms are shown. And each arm has nk joints.


### 2.2. Product of the Exponential Formula

The POE formula describes the kinematical relationship by using the exponential form of screws. Only two frames are needed, the tool frameT and the referential frame S. The formula is written as
(1)gTSθ=eθ1ξ^1eθ2ξ^2Leθnξ^ngTS0,where gTSθ and gTS0 represent the final and initial poses of frame T relative to frame S, respectively. eθiξ^i is just the exponential form of rigid motion. ξi defines the twist of the corresponding joint. For a revolute joint,
(2)ξi=ωi,ri×ωiT,where ωi is the unit vector along the axis of the twist and ri is the position vector of an arbitrary point on the axis.gTS can also be expressed as
(3)gTS=RTS,pS∈SE3,where RTS is the rotational matrix and pS is the position vector.gTS satisfies an important rule:
(4)gTA=gSAgTS.It means the poses of frameT relative to frame S and frame A can be transformed mutually.


## 2.1. The Model of the Space-Robot System

A space-robot system consists of the base (satellite) and arms (also called manipulators, single or multiple) typically. Each arm consists of several links, joints, and an end-effector (EE). The following assumptions are made to build a concise and accurate model.(1)
The position and orientation of the base is not controlled, and there are no external forces on the system, which means the system is at free-floating mode
(2)
Each arm is a single chain connected to the base, and the parallel manipulator is not considered
(3)
All links and joints are rigid
(4)
All joints are revolute, and all the rotation angles are limited between−180∘ and 180∘
(5)
The number of dofs of each arm is no greater than six. For a planar model, it is no greater than three. A redundant system is not consideredA model is provided in Figure1. The definitions of each symbol are given as follows.
(i)
∑I represents the inertial frame and is located at the centroid of the whole system
(ii)
∑B represents the base-fixed frame
(iii)
VectorpI0 represents the centroidal position of the base in the inertial frame
(iv)
VectorpIik represents the centroidal position of the kth arm’s ith link in the inertial frame
(v)
VectorpIEk represents the position of the kth arm’s EE in the inertial frameFigure 1
The model of a space-robot system. The system hasN arms: only the 1th, kth, and Nth arms are shown. And each arm has nk joints.


## 2.2. Product of the Exponential Formula

The POE formula describes the kinematical relationship by using the exponential form of screws. Only two frames are needed, the tool frameT and the referential frame S. The formula is written as
(1)gTSθ=eθ1ξ^1eθ2ξ^2Leθnξ^ngTS0,where gTSθ and gTS0 represent the final and initial poses of frame T relative to frame S, respectively. eθiξ^i is just the exponential form of rigid motion. ξi defines the twist of the corresponding joint. For a revolute joint,
(2)ξi=ωi,ri×ωiT,where ωi is the unit vector along the axis of the twist and ri is the position vector of an arbitrary point on the axis.gTS can also be expressed as
(3)gTS=RTS,pS∈SE3,where RTS is the rotational matrix and pS is the position vector.gTS satisfies an important rule:
(4)gTA=gSAgTS.It means the poses of frameT relative to frame S and frame A can be transformed mutually.


## 3. Kinematics Deduction Base on Screw Theory

### 3.1. Forward Kinematics

In this section, forward kinematical equations of the above-mentioned model are established.According to the POE formula, the final pose of the centroidal frame of thekth arm’s ith link in the base-fixed frame can be given by
(5)gCikB=eθ1kξ^1keθ2kξ^2kLeθikξ^ikgCikB0=RBikpBik01,i=1,2,K,nk,k=1,2,⋯,N,and the final pose of the kth arm’s EE can be given by
(6)gEkB=eθ1kξ^1keθ2kξ^2k⋯eθnkξ^nkgEkB0=RBEkpBEk01.The final pose of the base and centroidal frame of thekth arm’s ith link in the inertial frame can be expressed as
(7)gBI=RI0pI001,gCikI=RIikpIik01,where RI0 is determined by the orientation of the base. If we define the attitude angles (roll, pitch, and yaw) of the base as Ψ=δ,β,γT, then
(8)RI0=cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ,where c means cos and s means sin.According to the transformation rule of rigid motion, there is(9)gCikI=gBIgCikB=RI0pI001RBikpBik01=RI0RBikRI0pBik+pI001.So,(10)RIik=RI0RBik,pIi=RI0pBik+pI0,and the final pose of the kth arm’s EE in the inertial frame can be expressed as
(11)gEkI=gBIgEkB=RI0RBEkRI0pBEk+pI001.In the inertial frame, the centroidal formula of the system is written as(12)m0pI0+∑k=1N∑i=1nkmikpIik=MpIC=0.Substituting equation (10) into equation (12) yields
(13)pI0=−RI0M∑k=1N∑i=1nkmikpBik.Based on these deductions,gEkI can be calculated while the known variables are changing.Situation 1.
When both the position and orientation of the base are measurable, equation (11) can be used directly.Situation 2.
When only the orientation of the base can be obtained, substituting equation (13) into equation (11), there is
(14)gEkI=RI0RBEkRI0pBEk−RI0M∑k=1N∑i=1nkmikpBik01,where pBik can be calculated by equation (5).Situation 3.
When only the position of the base can be obtained, substituting equation (8) into equation (13), there is
(15)pI0=−cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ1M∑k=1N∑i=1nkmikpBik.
LetpI0=p01,p02,p03T,1/M∑k=1N∑i=1nkmikpBik=pt=pt1,pt2,pt3T, then equation (15) can be written as
(16)cβcγpt1+sδsβcγ−cδsγpt2+cδsβcγ+sδsγpt3+p01=0,cβsγpt1+sδsβsγ+cδcγpt2+cδsβsγ−sδcγpt3+p02=0,−sβpt1+sδcβpt2+cδcβpt3+p03=0.
Ψ=δ,β,γ can be calculated using this equation, then gEkI can be calculated by equation (11).


### 3.2. Inverse Kinematics

Firstly, the feasibility of the inverse solution should be demonstrated. The number of state variables of the system is6+∑k=1Nnk, including the position and orientation of the base. The number of equations is 6N, which is equal to the total number of state parameters of all the EEs. The formula of conservation of linear momentum is a holonomic constraint, which can eliminate three independent state variables of the system. If the other three state variables are known, the number of unknown state variables will be no greater than the number of equations ∑k=1Nnk≤6N. Then, the equations will have limited numbers of solutions. For the planar model, the explanation is similar. Accordingly, the following situations with different known variables are discussed.Situation 1.
When both the position and the orientation of the base are measurable, substituting equation (6) into equation (11), there is
(17)gEkI=gBIgEkB=gBIeθ1kξ^1keθ2kξ^2kLeθnkξ^nkgEkB0.
Let,(18)G=eθ1kξ^1keθ2kξ^2kLeθnkξ^nk=gBI−1gEkIgEkB0−1.
Joint angles of thekth arm θk=θik can be solved by using the Paden-Kahan subproblem method. It will not be discussed further because it belongs to a maturely studied subject.Situation 2.
When only the orientation of the base can be obtained, equation (14) can be converted into the following form:
(19)RI0RBEkRI0pBEk−RI0M∑k=1N∑i=1nkmikpBik01−gEkI=Ykθ−gEkI=0,where Yk∈SE3. Twelve elements of Yk are dependent on the joint angles. Considering that nine elements of the rotational matrix are dependent mutually, let
(20)Yk=y11ky12ky13ky14ky21ky22ky23ky24ky31ky32ky33ky34k01,gEkI=t11kt12kt13kt14kt21kt22kt23kt24kt31kt32kt33kt34k01.
By using the unit quaternion method, equation (19) can be simplified to
(21)f1kθ=121+y11k+y22k+y33k−121+t11k+t22k+t33k=0,f2kθ=y32k−y23k−t32k−t23k=0,f3kθ=y13k−y31k−t13k−t31k=0,f4kθ=y21k−y12k−t21k−t12k=0,f5kθ=y14k−t14k=0,f6kθ=y24k−t24k=0,f7kθ=y34k−t34k=0.
All the equations of all the arms compose the following equation set:(22)Fθ=0;θ=θik,i=1,2,K,nk,k=1,2,⋯,N.
All joint anglesθ can be obtained by solving this equation set. However, it may be hard to get a closed-form solution due to its complexity. A numerical approach is provided based on Newton’s iteration method [22]. Firstly, the Jacobian matrix of the equation set is given by
(23)JFθ=∂f11∂θ11∂f11∂θ21⋯∂f11∂θ12⋯∂f11∂θnNN∂f21∂θ11∂f21∂θ21⋯∂f21∂θ12⋯∂f21∂θnNN⋮⋮⋱⋮⋱⋮∂f12∂θ11∂f12∂θ21⋯∂f12∂θ12⋯∂f12∂θnNN⋮⋮⋱⋮⋱⋮∂f7N∂θ11∂f7N∂θ21⋯∂f7N∂θ12⋯∂f7N∂θnNN.
JFθ∈R7N×Ntotal, where Ntotal=∑k=1Nnk. Then, the iterative formula can be written as
(24)θi+1=θi−JF+Fθ,where JF+ is the pseudoinverse matrix of JF.Situation 3.
When only the position of the base can be obtained, the problem is more complex. Firstly, to convert equation (9) into the following form:
(25)RI0RBEkRI0pBEk−RI0M∑k=1N∑i=1nkmikpBik01−gEkI=Zkθ,Ψ−gEkI=0,let,
(26)Zk=z11kz12kz13kz14kz21kz22kz23kz24kz31kz32kz33kz34k01.
Similarly, equation (25) can be simplified to
(27)g1kθ,Ψ=121+z11k+z22k+z33k−121+t11k+t22k+t33k=0,g2kθ,Ψ=z32k−z23k−t32k−t23k=0,g3kθ,Ψ=z13k−z31k−t13k−t31k=0,g4kθ,Ψ=z21k−z12k−t21k−t12k=0,g5kθ,Ψ=z14k−t14k=0,g6kθ,Ψ=z24k−t24k=0,g7kθ,Ψ=z34k−t34k=0.
Equation (16) can be converted into the following form:
(28)g1′θ,Ψ=cβcγpt1+sδsβcγ−cδsγpt2+cδsβcγ+sδsγpt3+p01=0,g2′θ,Ψ=cβsγpt1+sδsβsγ+cδcγpt2+cδsβsγ−sδcγpt3+p02=0,g3′θ,Ψ=−sβpt1+sδcβpt2+cδcβpt3+p03=0.
All the equations of all the arms and equation (28) compose another equation set:
(29)Gφ=0;φ=θik,δ,β,γ,i=1,2,K,nk,k=1,2,⋯,N.
The Jacobian matrix of the equation set is given by(30)JGφ=∂g11∂θ11∂g11∂θ21⋯∂g11∂θ12⋯∂g11∂θnNN∂g11∂δ∂g11∂β∂g11∂γ∂g21∂θ11∂g21∂θ21⋯∂g21∂θ12⋯∂g21∂θnNN∂g21∂δ∂g21∂β∂g21∂γ⋮⋮⋱⋮⋱⋮⋮⋮⋮∂g12∂θ11∂g12∂θ21⋯∂g12∂θ12⋯∂g12∂θnNN∂g12∂δ∂g12∂β∂g12∂γ⋮⋮⋱⋮⋱⋮⋮⋮⋮∂g7N∂θ11∂g7N∂θ21⋯∂g7N∂θ12⋯∂g7N∂θnNN∂g7N∂δ∂g7N∂β∂g7N∂γ∂g1′∂θ11∂g1′∂θ21⋯∂g1′∂θ12⋯∂g1′∂θnNN∂g1′∂δ∂g1′∂β∂g1′∂γ∂g2′∂θ11∂g2′∂θ21⋯∂g2′∂θ12⋯∂g2′∂θnNN∂g2′∂δ∂g2′∂β∂g2′∂γ∂g3′∂θ11∂g3′∂θ21⋯∂g3′∂θ12⋯∂g3′∂θnNN∂g3′∂δ∂g3′∂β∂g3′∂γ.
JGφ∈R7N+3×Ntotal+3. The iterative formula is written as
(31)φi+1=φi−JG+Gφ,where JG+ is the pseudoinverse matrix of JG.In particular, all the equations in this section can be simplified into kinematical equations of a single-arm space-robot system whenN=1.


## 3.1. Forward Kinematics

In this section, forward kinematical equations of the above-mentioned model are established.According to the POE formula, the final pose of the centroidal frame of thekth arm’s ith link in the base-fixed frame can be given by
(5)gCikB=eθ1kξ^1keθ2kξ^2kLeθikξ^ikgCikB0=RBikpBik01,i=1,2,K,nk,k=1,2,⋯,N,and the final pose of the kth arm’s EE can be given by
(6)gEkB=eθ1kξ^1keθ2kξ^2k⋯eθnkξ^nkgEkB0=RBEkpBEk01.The final pose of the base and centroidal frame of thekth arm’s ith link in the inertial frame can be expressed as
(7)gBI=RI0pI001,gCikI=RIikpIik01,where RI0 is determined by the orientation of the base. If we define the attitude angles (roll, pitch, and yaw) of the base as Ψ=δ,β,γT, then
(8)RI0=cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ,where c means cos and s means sin.According to the transformation rule of rigid motion, there is(9)gCikI=gBIgCikB=RI0pI001RBikpBik01=RI0RBikRI0pBik+pI001.So,(10)RIik=RI0RBik,pIi=RI0pBik+pI0,and the final pose of the kth arm’s EE in the inertial frame can be expressed as
(11)gEkI=gBIgEkB=RI0RBEkRI0pBEk+pI001.In the inertial frame, the centroidal formula of the system is written as(12)m0pI0+∑k=1N∑i=1nkmikpIik=MpIC=0.Substituting equation (10) into equation (12) yields
(13)pI0=−RI0M∑k=1N∑i=1nkmikpBik.Based on these deductions,gEkI can be calculated while the known variables are changing.Situation 1.
When both the position and orientation of the base are measurable, equation (11) can be used directly.Situation 2.
When only the orientation of the base can be obtained, substituting equation (13) into equation (11), there is
(14)gEkI=RI0RBEkRI0pBEk−RI0M∑k=1N∑i=1nkmikpBik01,where pBik can be calculated by equation (5).Situation 3.
When only the position of the base can be obtained, substituting equation (8) into equation (13), there is
(15)pI0=−cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ1M∑k=1N∑i=1nkmikpBik.
LetpI0=p01,p02,p03T,1/M∑k=1N∑i=1nkmikpBik=pt=pt1,pt2,pt3T, then equation (15) can be written as
(16)cβcγpt1+sδsβcγ−cδsγpt2+cδsβcγ+sδsγpt3+p01=0,cβsγpt1+sδsβsγ+cδcγpt2+cδsβsγ−sδcγpt3+p02=0,−sβpt1+sδcβpt2+cδcβpt3+p03=0.
Ψ=δ,β,γ can be calculated using this equation, then gEkI can be calculated by equation (11).


## 3.2. Inverse Kinematics

Firstly, the feasibility of the inverse solution should be demonstrated. The number of state variables of the system is6+∑k=1Nnk, including the position and orientation of the base. The number of equations is 6N, which is equal to the total number of state parameters of all the EEs. The formula of conservation of linear momentum is a holonomic constraint, which can eliminate three independent state variables of the system. If the other three state variables are known, the number of unknown state variables will be no greater than the number of equations ∑k=1Nnk≤6N. Then, the equations will have limited numbers of solutions. For the planar model, the explanation is similar. Accordingly, the following situations with different known variables are discussed.Situation 1.
When both the position and the orientation of the base are measurable, substituting equation (6) into equation (11), there is
(17)gEkI=gBIgEkB=gBIeθ1kξ^1keθ2kξ^2kLeθnkξ^nkgEkB0.
Let,(18)G=eθ1kξ^1keθ2kξ^2kLeθnkξ^nk=gBI−1gEkIgEkB0−1.
Joint angles of thekth arm θk=θik can be solved by using the Paden-Kahan subproblem method. It will not be discussed further because it belongs to a maturely studied subject.Situation 2.
When only the orientation of the base can be obtained, equation (14) can be converted into the following form:
(19)RI0RBEkRI0pBEk−RI0M∑k=1N∑i=1nkmikpBik01−gEkI=Ykθ−gEkI=0,where Yk∈SE3. Twelve elements of Yk are dependent on the joint angles. Considering that nine elements of the rotational matrix are dependent mutually, let
(20)Yk=y11ky12ky13ky14ky21ky22ky23ky24ky31ky32ky33ky34k01,gEkI=t11kt12kt13kt14kt21kt22kt23kt24kt31kt32kt33kt34k01.
By using the unit quaternion method, equation (19) can be simplified to
(21)f1kθ=121+y11k+y22k+y33k−121+t11k+t22k+t33k=0,f2kθ=y32k−y23k−t32k−t23k=0,f3kθ=y13k−y31k−t13k−t31k=0,f4kθ=y21k−y12k−t21k−t12k=0,f5kθ=y14k−t14k=0,f6kθ=y24k−t24k=0,f7kθ=y34k−t34k=0.
All the equations of all the arms compose the following equation set:(22)Fθ=0;θ=θik,i=1,2,K,nk,k=1,2,⋯,N.
All joint anglesθ can be obtained by solving this equation set. However, it may be hard to get a closed-form solution due to its complexity. A numerical approach is provided based on Newton’s iteration method [22]. Firstly, the Jacobian matrix of the equation set is given by
(23)JFθ=∂f11∂θ11∂f11∂θ21⋯∂f11∂θ12⋯∂f11∂θnNN∂f21∂θ11∂f21∂θ21⋯∂f21∂θ12⋯∂f21∂θnNN⋮⋮⋱⋮⋱⋮∂f12∂θ11∂f12∂θ21⋯∂f12∂θ12⋯∂f12∂θnNN⋮⋮⋱⋮⋱⋮∂f7N∂θ11∂f7N∂θ21⋯∂f7N∂θ12⋯∂f7N∂θnNN.
JFθ∈R7N×Ntotal, where Ntotal=∑k=1Nnk. Then, the iterative formula can be written as
(24)θi+1=θi−JF+Fθ,where JF+ is the pseudoinverse matrix of JF.Situation 3.
When only the position of the base can be obtained, the problem is more complex. Firstly, to convert equation (9) into the following form:
(25)RI0RBEkRI0pBEk−RI0M∑k=1N∑i=1nkmikpBik01−gEkI=Zkθ,Ψ−gEkI=0,let,
(26)Zk=z11kz12kz13kz14kz21kz22kz23kz24kz31kz32kz33kz34k01.
Similarly, equation (25) can be simplified to
(27)g1kθ,Ψ=121+z11k+z22k+z33k−121+t11k+t22k+t33k=0,g2kθ,Ψ=z32k−z23k−t32k−t23k=0,g3kθ,Ψ=z13k−z31k−t13k−t31k=0,g4kθ,Ψ=z21k−z12k−t21k−t12k=0,g5kθ,Ψ=z14k−t14k=0,g6kθ,Ψ=z24k−t24k=0,g7kθ,Ψ=z34k−t34k=0.
Equation (16) can be converted into the following form:
(28)g1′θ,Ψ=cβcγpt1+sδsβcγ−cδsγpt2+cδsβcγ+sδsγpt3+p01=0,g2′θ,Ψ=cβsγpt1+sδsβsγ+cδcγpt2+cδsβsγ−sδcγpt3+p02=0,g3′θ,Ψ=−sβpt1+sδcβpt2+cδcβpt3+p03=0.
All the equations of all the arms and equation (28) compose another equation set:
(29)Gφ=0;φ=θik,δ,β,γ,i=1,2,K,nk,k=1,2,⋯,N.
The Jacobian matrix of the equation set is given by(30)JGφ=∂g11∂θ11∂g11∂θ21⋯∂g11∂θ12⋯∂g11∂θnNN∂g11∂δ∂g11∂β∂g11∂γ∂g21∂θ11∂g21∂θ21⋯∂g21∂θ12⋯∂g21∂θnNN∂g21∂δ∂g21∂β∂g21∂γ⋮⋮⋱⋮⋱⋮⋮⋮⋮∂g12∂θ11∂g12∂θ21⋯∂g12∂θ12⋯∂g12∂θnNN∂g12∂δ∂g12∂β∂g12∂γ⋮⋮⋱⋮⋱⋮⋮⋮⋮∂g7N∂θ11∂g7N∂θ21⋯∂g7N∂θ12⋯∂g7N∂θnNN∂g7N∂δ∂g7N∂β∂g7N∂γ∂g1′∂θ11∂g1′∂θ21⋯∂g1′∂θ12⋯∂g1′∂θnNN∂g1′∂δ∂g1′∂β∂g1′∂γ∂g2′∂θ11∂g2′∂θ21⋯∂g2′∂θ12⋯∂g2′∂θnNN∂g2′∂δ∂g2′∂β∂g2′∂γ∂g3′∂θ11∂g3′∂θ21⋯∂g3′∂θ12⋯∂g3′∂θnNN∂g3′∂δ∂g3′∂β∂g3′∂γ.
JGφ∈R7N+3×Ntotal+3. The iterative formula is written as
(31)φi+1=φi−JG+Gφ,where JG+ is the pseudoinverse matrix of JG.In particular, all the equations in this section can be simplified into kinematical equations of a single-arm space-robot system whenN=1.


## 4. Examples and Discussion

### 4.1. Solution of the Planar Model

Firstly, a simulation example of a three-link dual-arm planar robot is given in this part. The model at the initial configuration is shown in Figure2.Figure 2
The model of a three-link dual-arm planar robot at the initial configuration.To make a comparison between the simulation results of MATLAB and Adams, a simplified model of the robot is built in Adams and a certain motion process (from the initial configuration to the final configuration) is run. The final configuration of the Adams model is shown in Figure3.Figure 3
The final configuration of the Adams model.Detailed parameters of the model are given as follows:(32)ω11=ω21=ω31=ω12=ω22=ω32=0,0,1T,r11=1.2,0.7,0T,r21=3.0,0.7,0T,r31=4.8,0.7,0Tr12=−1.2,0.7,0T,r22=−3.0,0.7,0T,r32=−4.8,0.7,0T.So,(33)ξ11=ω11,r11×ω11T=0,0,1,0.7,−1.2,0T,ξ21=ω21,r21×ω21T=0,0,1,0.7,−3,0T,ξ31=ω31,r31×ω31T=0,0,1,0.7,−4.8,0T,ξ12=ω12,r12×ω12T=0,0,1,0.7,1.2,0T,ξ22=ω22,r22×ω22T=0,0,1,0.7,3,0T,ξ32=ω32,r32×ω32T=0,0,1,0.7,4.8,0T,and m0=400,m11=18,m21=18,m31=24,m12=18,m22=18,m32=24, and M=520.
(34)gC11B0=2.1E3×30.7001,gC21B0=3.9E3×30.7001,gC31B0=5.7E3×30.7001,gE1B0=6.6E3×30.7001,gC12B0=−2.1E3×30.7001,gC22B0=−3.9E3×30.7001,gC32B0=−5.7E3×30.7001,gE2B0=−6.6E3×30.7001.


#### 4.1.1. Forward Kinematical Solution of the Planar Model

Base on the method in Section3.1, forward kinematical solution of this planar model is given.Final joint angles areθ=83.15°,18.94°,128.4°,−37.19°,−29.44°,−51.09°T. Then, according to equation (5), there are
(35)gC11B=RB11pB1101=eθ11ξ^11gC11B0=0.1193−0.992901.30730.99290.119301.5936001001,gC21B=RB21pB2101=eθ21ξ^21eθ21ξ^21gC21B0=−0.2094−0.977801.22620.9778−0.209401.3672001001,gC31B=RB31pB3101=eθ31ξ^31eθ31ξ^31eθ31ξ^31gC31B0=−0.63620.771500.4647−0.7715−0.636203.5511001001,gC12B=RB12pB1201=eθ12ξ^12gC12B0=0.79660.60450−1.9170−0.60450.796601.2440001001,gC22B=RB22pB2201=eθ22ξ^22eθ22ξ^22gC22B0=0.39670.91800−2.9909−0.91800.396702.6142001001,gC32B=RB32pB3201=eθ32ξ^32eθ32ξ^32eθ32ξ^32gC32B0=−0.46520.88520−2.9293−0.8852−0.465204.2371001001.According to equation (6), there are
(36)E1Bg=RBE1pBE101=eθ11ξ^11eθ21ξ^21eθ31ξ^31gE1B0=−0.63620.77150−0.1079−0.7715−0.636202.8568001001,E2Bg=RBE2pBE201=eθ12ξ^12eθ22ξ^22eθ32ξ^32gE2B0=‐0.46520.88520‐2.5107−0.8852‐0.465205.0338001001.Situation 1.
Both the position and the orientation of the base are known:(37)pI0=0.1470,−0.6772,0T,Ψ=0,0,−4.18∘T.
So,(38)gBI=0.99730.072900.1470−0.07290.99730−0.6772001001.
According to equation (11), there are
(39)gE1I=gBIgE1B=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=gBIgE2B=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.Situation 2.
Only the orientation of the base is obtained:Ψ=0,0,−4.18∘T.
So,(40)RI0=0.99730.07290−0.07290.99730001.
According to equation (14), there are
(41)gE1I=RI0RBE1RI0pBE1−RI0M∑k=12∑i=13mikpBik01=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=RI0RBE2RI0pBE2−RI0M∑k=12∑i=13mikpBik01=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.Situation 3.
Only the position of the base is obtained:pI0=0.1470,−0.6772,0T.
Based on the known parameters, there is(42)pt=1M∑k=12∑i=13mikpBik=−0.1959,0.6647,0T.
According to equation (16), there is
(43)−0.1959cγ−0.6647sγ+0.1470=0,−0.1959sγ+0.6647cγ−0.6772=0.
So,γ=−0.0729=−4.17∘. Then,
(44)gBI=0.99740.072700.1470−0.07270.99740−0.6772001001.
According to equation (11), there are
(45)gE1I=gBIgE1B=−0.69060.723200.2472−0.7232−0.690602.1798001001,gE2I=gBIgE2B=‐0.52830.84910‐1.9909‐0.8491‐0.528304.5258001001.Comparing the results of these situations, which are almost equal to each other, it can be concluded that the method is feasible and effective. The result of Situation 3 has a subtle error due to the truncation error of angles during the calculation.


#### 4.1.2. Inverse Kinematical Solution of the Planar Model

Based on the method in Section3.2, iterative programs for each situation are written with MATLAB and are run separately. Firstly, the final pose of the two arms is given by
(46)gE1I=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.The process of the iterative programs is as follow.Step 1.
Set the calculating parameters and known variables. Max number of iterations isN=100. Convergence tolerance is ε=0.001. Known parameters are Ψ=0,0,−4.18∘T (for Situation 2). pI0=0.1469,−0.6773,0T (for Situation 3).Step 2.
Set initial joint angles.θ0=60°,30°,80°,−60°,−30°,−80°T. For the program of Situation 3, the initial attitude angles of the base should also be given: Ψ0=0,0,−10°T.Step 3.
Calculate the Jacobian matrix of each iterative step using equation (23) (for Situation 2) or equation (30) (for Situation 3).Step 4.
Calculate the angles of each iterative step using equation (24) (for Situation 2) or equation (31) (for Situation 3).Step 5.
Calculate the residual error and judge whether the iteration is over. If yes, go to the next step. If no, go to step 2.Step 6.
Calculate the final pose of each arm using the iterative result of the angles.The results of the Adams simulation and the iterations are listed in Table1.Table 1
Simulation results of the Adams simulation and the iterations for the planar model.
Method
Joint angles (°)
Base’s attitude angles
Max error
Adams
83.15,18.94,128.4,−37.19,−29.44,−51.09T
0,0,−4.18°T
—
Iteration for Situation 2
83.096,19.014,128.351,−37.185,−29.449,−51.105T
—
0.39%
Iteration for Situation 3
83.089,19.020,128.364,−37.214,−29.443,−51.077T
0,0,−4.163°T
0.42%The comparison shows that the results of the iterations are quite approximate to the real solution, which demonstrates the accuracy of the proposed methods. Actually, comparison between forward kinematical solutions and results of the iterations could also verify the accuracy.Furthermore, efficiency of the methods is studied. Iterative time and number of iterative steps under certain calculating conditions are recorded while the initial parameters of the iteration of Situation 2 are changing. The results are presented in Table2.Table 2
Simulation results of iterative time and number of iterative steps.
Initial joint angles (°)
Iterative time (s)
Steps
100°,50°,60°,−80°,−60°,−100°T
5.94
11
60°,30°,80°,−60°,−30°,−80°T
3.41
6
65°,25°,95°,−55°,−30°,−60°T
3.51
6
70°,23°,105°,−45°,−30°,−55°T
2.97
5
75°,20°,115°,−40°,−30°,−50°T
2.47
4The results show that the iterative method is fairly efficient, and it could perform better when the initial parameters are closer to the real values. This is determined by the inherent property of Newton’s iterative method.


### 4.2. Solution of the Spatial Model

To verify the application of the proposed method to a tridimensional model, a calculating example of a single-arm three-link space robot is presented. The Adams model at the initial configuration and the final configuration is shown in Figures4 and 5, respectively.Figure 4
The Adams model of the single-arm three-link space robot at the initial configuration.Figure 5
The Adams model of the single-arm three-link space robot at the final configuration.The parameters of this spatial model are given as follows:(47)ω1=ω3=0,0,1T,ω2=0,−1,0Tr1=1.2030,0.6929,0T,r2=2.903,0.6929,0T,r3=4.603,0.6929,0TSo,(48)ξ1=ω1,r1×ω1T=0,0,1,0.6929,−1.2030,0T,ξ2=ω2,r2×ω2T=0,−1,0,0,0,−2.9030T,ξ3=ω3,r3×ω3T=0,0,1,0.6929,−4.603,0T,and m0=400,m1=15,m2=15,m3=24, and M=454.
(49)gC1B0=2.275E3×30.6929001,gC2B0=3.975E3×30.6929001,gC3B0=5.675E3×30.6929001,EBg0=6.306E3×30.6829001.


#### 4.2.1. Forward Kinematical Solution of the Spatial Model

The final joint angles areθ=−67.51°,88.35°,21.62°T. According to equation (5), there are
(50)gC1B=RB1pB101=eθ1ξ^1gC1B0=0.38250.923901.6131−0.92390.38250−0.2976001001,gC2B=RB2pB201=eθ1ξ^1eθ2ξ^2gC2B0=0.01100.9239−0.38241.8651−0.02660.38250.9236−0.90630.999600.02881.071601,gC3B=RB3pB301=eθ1ξ^1eθ2ξ^2eθ3ξ^3gC3B0=0.35070.8549−0.38242.24790.11620.36540.9236−0.79850.9293−0.36830.02882.695501.According to equation (6), there is
(51)gEB=RBEkpBEk01=eθ1ξ^1eθ2ξ^2eθ3ξ^3gEB0=0.35070.8549−0.38242.46020.11620.36540.9236−0.72900.9293−0.36830.02883.285701.Situation 1.
Both the position and the orientation of the base are known:pI0=−0.3029,−0.0151,−0.0319T,Ψ=11.39°,27.60°,25.23°T.
So,(52)gBI=0.8017−0.33510.4950−0.30290.37770.92580.0149−0.0151−0.46330.17500.8688−0.031901.
According to equation (11), there is
(53)gEI=gBIgEB=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.Situation 2.
Only the orientation of the base is obtained:Ψ=11.39°,27.60°,25.23°T.
So,(54)RI0=0.8017−0.33510.49500.37770.92580.0149−0.46330.17500.8688.
According to equation (14), there is
(55)gEI=RI0RBERI0pBE−RI0M∑i=13mipBi01=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.Situation 3.
Only the position of the base is obtained:pI0=−0.3029,−0.0151,−0.0319T.
(56)pt=1Mm1pB1,+m2pB2+m3pB3=0.2338,−0.0820,0.1779T.
According to equation (16), there is
(57)cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ0.2338−0.08200.1779+−0.3029−0.0151−0.0319=0.
So,Ψ=δ,β,γT=11.385°,27.595°,25.236°T. Then,
(58)gBI=0.8017−0.33520.4949−0.30290.37780.92570.0150−0.0151−0.46320.17490.8688−0.031901.
According to equation (11), there is
(59)gEI=gBIgEB=0.70210.3805−0.60193.53990.25410.65580.71090.28910.6652−0.65200.36371.555601.


#### 4.2.2. Inverse Kinematical Solution of the Spatial Model

Likewise, final pose of EE and the iteration parameters are given at first.(60)gEI=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.The max number of iterations isN=100. Convergence tolerance is ε=0.001. Known parameters are Ψ=11.39°,27.60°,25.23°T (for Situation 2). pI0=−0.3029,−0.0151,−0.0319T (for Situation 3).The initial joint angles areθ0=45°,45°,45°T and Ψ0=10,10,10∘T (for Situation 3).The results are listed in Table3. Accuracy of the proposed iterative methods is demonstrated while applying to the tridimensional model.Table 3
Simulation results of the Adams simulation and the iterations for the spatial model.
Method
Joint angles
Base’s attitude angles
Max error
Adams
−67.51°,88.35°,21.62°T
11.39°,27.60°,25.23°T
—
Iteration for Situation 2
−67.504°,88.332°,21.645°T
—
0.12%
Iteration for Situation 3
−67.263°,88.665°,21.874°T
11.590°,27.577°,25.065°T
1.73%


## 4.1. Solution of the Planar Model

Firstly, a simulation example of a three-link dual-arm planar robot is given in this part. The model at the initial configuration is shown in Figure2.Figure 2
The model of a three-link dual-arm planar robot at the initial configuration.To make a comparison between the simulation results of MATLAB and Adams, a simplified model of the robot is built in Adams and a certain motion process (from the initial configuration to the final configuration) is run. The final configuration of the Adams model is shown in Figure3.Figure 3
The final configuration of the Adams model.Detailed parameters of the model are given as follows:(32)ω11=ω21=ω31=ω12=ω22=ω32=0,0,1T,r11=1.2,0.7,0T,r21=3.0,0.7,0T,r31=4.8,0.7,0Tr12=−1.2,0.7,0T,r22=−3.0,0.7,0T,r32=−4.8,0.7,0T.So,(33)ξ11=ω11,r11×ω11T=0,0,1,0.7,−1.2,0T,ξ21=ω21,r21×ω21T=0,0,1,0.7,−3,0T,ξ31=ω31,r31×ω31T=0,0,1,0.7,−4.8,0T,ξ12=ω12,r12×ω12T=0,0,1,0.7,1.2,0T,ξ22=ω22,r22×ω22T=0,0,1,0.7,3,0T,ξ32=ω32,r32×ω32T=0,0,1,0.7,4.8,0T,and m0=400,m11=18,m21=18,m31=24,m12=18,m22=18,m32=24, and M=520.
(34)gC11B0=2.1E3×30.7001,gC21B0=3.9E3×30.7001,gC31B0=5.7E3×30.7001,gE1B0=6.6E3×30.7001,gC12B0=−2.1E3×30.7001,gC22B0=−3.9E3×30.7001,gC32B0=−5.7E3×30.7001,gE2B0=−6.6E3×30.7001.


### 4.1.1. Forward Kinematical Solution of the Planar Model

Base on the method in Section3.1, forward kinematical solution of this planar model is given.Final joint angles areθ=83.15°,18.94°,128.4°,−37.19°,−29.44°,−51.09°T. Then, according to equation (5), there are
(35)gC11B=RB11pB1101=eθ11ξ^11gC11B0=0.1193−0.992901.30730.99290.119301.5936001001,gC21B=RB21pB2101=eθ21ξ^21eθ21ξ^21gC21B0=−0.2094−0.977801.22620.9778−0.209401.3672001001,gC31B=RB31pB3101=eθ31ξ^31eθ31ξ^31eθ31ξ^31gC31B0=−0.63620.771500.4647−0.7715−0.636203.5511001001,gC12B=RB12pB1201=eθ12ξ^12gC12B0=0.79660.60450−1.9170−0.60450.796601.2440001001,gC22B=RB22pB2201=eθ22ξ^22eθ22ξ^22gC22B0=0.39670.91800−2.9909−0.91800.396702.6142001001,gC32B=RB32pB3201=eθ32ξ^32eθ32ξ^32eθ32ξ^32gC32B0=−0.46520.88520−2.9293−0.8852−0.465204.2371001001.According to equation (6), there are
(36)E1Bg=RBE1pBE101=eθ11ξ^11eθ21ξ^21eθ31ξ^31gE1B0=−0.63620.77150−0.1079−0.7715−0.636202.8568001001,E2Bg=RBE2pBE201=eθ12ξ^12eθ22ξ^22eθ32ξ^32gE2B0=‐0.46520.88520‐2.5107−0.8852‐0.465205.0338001001.Situation 1.
Both the position and the orientation of the base are known:(37)pI0=0.1470,−0.6772,0T,Ψ=0,0,−4.18∘T.
So,(38)gBI=0.99730.072900.1470−0.07290.99730−0.6772001001.
According to equation (11), there are
(39)gE1I=gBIgE1B=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=gBIgE2B=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.Situation 2.
Only the orientation of the base is obtained:Ψ=0,0,−4.18∘T.
So,(40)RI0=0.99730.07290−0.07290.99730001.
According to equation (14), there are
(41)gE1I=RI0RBE1RI0pBE1−RI0M∑k=12∑i=13mikpBik01=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=RI0RBE2RI0pBE2−RI0M∑k=12∑i=13mikpBik01=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.Situation 3.
Only the position of the base is obtained:pI0=0.1470,−0.6772,0T.
Based on the known parameters, there is(42)pt=1M∑k=12∑i=13mikpBik=−0.1959,0.6647,0T.
According to equation (16), there is
(43)−0.1959cγ−0.6647sγ+0.1470=0,−0.1959sγ+0.6647cγ−0.6772=0.
So,γ=−0.0729=−4.17∘. Then,
(44)gBI=0.99740.072700.1470−0.07270.99740−0.6772001001.
According to equation (11), there are
(45)gE1I=gBIgE1B=−0.69060.723200.2472−0.7232−0.690602.1798001001,gE2I=gBIgE2B=‐0.52830.84910‐1.9909‐0.8491‐0.528304.5258001001.Comparing the results of these situations, which are almost equal to each other, it can be concluded that the method is feasible and effective. The result of Situation 3 has a subtle error due to the truncation error of angles during the calculation.


### 4.1.2. Inverse Kinematical Solution of the Planar Model

Based on the method in Section3.2, iterative programs for each situation are written with MATLAB and are run separately. Firstly, the final pose of the two arms is given by
(46)gE1I=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.The process of the iterative programs is as follow.Step 1.
Set the calculating parameters and known variables. Max number of iterations isN=100. Convergence tolerance is ε=0.001. Known parameters are Ψ=0,0,−4.18∘T (for Situation 2). pI0=0.1469,−0.6773,0T (for Situation 3).Step 2.
Set initial joint angles.θ0=60°,30°,80°,−60°,−30°,−80°T. For the program of Situation 3, the initial attitude angles of the base should also be given: Ψ0=0,0,−10°T.Step 3.
Calculate the Jacobian matrix of each iterative step using equation (23) (for Situation 2) or equation (30) (for Situation 3).Step 4.
Calculate the angles of each iterative step using equation (24) (for Situation 2) or equation (31) (for Situation 3).Step 5.
Calculate the residual error and judge whether the iteration is over. If yes, go to the next step. If no, go to step 2.Step 6.
Calculate the final pose of each arm using the iterative result of the angles.The results of the Adams simulation and the iterations are listed in Table1.Table 1
Simulation results of the Adams simulation and the iterations for the planar model.
Method
Joint angles (°)
Base’s attitude angles
Max error
Adams
83.15,18.94,128.4,−37.19,−29.44,−51.09T
0,0,−4.18°T
—
Iteration for Situation 2
83.096,19.014,128.351,−37.185,−29.449,−51.105T
—
0.39%
Iteration for Situation 3
83.089,19.020,128.364,−37.214,−29.443,−51.077T
0,0,−4.163°T
0.42%The comparison shows that the results of the iterations are quite approximate to the real solution, which demonstrates the accuracy of the proposed methods. Actually, comparison between forward kinematical solutions and results of the iterations could also verify the accuracy.Furthermore, efficiency of the methods is studied. Iterative time and number of iterative steps under certain calculating conditions are recorded while the initial parameters of the iteration of Situation 2 are changing. The results are presented in Table2.Table 2
Simulation results of iterative time and number of iterative steps.
Initial joint angles (°)
Iterative time (s)
Steps
100°,50°,60°,−80°,−60°,−100°T
5.94
11
60°,30°,80°,−60°,−30°,−80°T
3.41
6
65°,25°,95°,−55°,−30°,−60°T
3.51
6
70°,23°,105°,−45°,−30°,−55°T
2.97
5
75°,20°,115°,−40°,−30°,−50°T
2.47
4The results show that the iterative method is fairly efficient, and it could perform better when the initial parameters are closer to the real values. This is determined by the inherent property of Newton’s iterative method.


## 4.1.1. Forward Kinematical Solution of the Planar Model

Base on the method in Section3.1, forward kinematical solution of this planar model is given.Final joint angles areθ=83.15°,18.94°,128.4°,−37.19°,−29.44°,−51.09°T. Then, according to equation (5), there are
(35)gC11B=RB11pB1101=eθ11ξ^11gC11B0=0.1193−0.992901.30730.99290.119301.5936001001,gC21B=RB21pB2101=eθ21ξ^21eθ21ξ^21gC21B0=−0.2094−0.977801.22620.9778−0.209401.3672001001,gC31B=RB31pB3101=eθ31ξ^31eθ31ξ^31eθ31ξ^31gC31B0=−0.63620.771500.4647−0.7715−0.636203.5511001001,gC12B=RB12pB1201=eθ12ξ^12gC12B0=0.79660.60450−1.9170−0.60450.796601.2440001001,gC22B=RB22pB2201=eθ22ξ^22eθ22ξ^22gC22B0=0.39670.91800−2.9909−0.91800.396702.6142001001,gC32B=RB32pB3201=eθ32ξ^32eθ32ξ^32eθ32ξ^32gC32B0=−0.46520.88520−2.9293−0.8852−0.465204.2371001001.According to equation (6), there are
(36)E1Bg=RBE1pBE101=eθ11ξ^11eθ21ξ^21eθ31ξ^31gE1B0=−0.63620.77150−0.1079−0.7715−0.636202.8568001001,E2Bg=RBE2pBE201=eθ12ξ^12eθ22ξ^22eθ32ξ^32gE2B0=‐0.46520.88520‐2.5107−0.8852‐0.465205.0338001001.Situation 1.
Both the position and the orientation of the base are known:(37)pI0=0.1470,−0.6772,0T,Ψ=0,0,−4.18∘T.
So,(38)gBI=0.99730.072900.1470−0.07290.99730−0.6772001001.
According to equation (11), there are
(39)gE1I=gBIgE1B=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=gBIgE2B=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.Situation 2.
Only the orientation of the base is obtained:Ψ=0,0,−4.18∘T.
So,(40)RI0=0.99730.07290−0.07290.99730001.
According to equation (14), there are
(41)gE1I=RI0RBE1RI0pBE1−RI0M∑k=12∑i=13mikpBik01=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=RI0RBE2RI0pBE2−RI0M∑k=12∑i=13mikpBik01=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.Situation 3.
Only the position of the base is obtained:pI0=0.1470,−0.6772,0T.
Based on the known parameters, there is(42)pt=1M∑k=12∑i=13mikpBik=−0.1959,0.6647,0T.
According to equation (16), there is
(43)−0.1959cγ−0.6647sγ+0.1470=0,−0.1959sγ+0.6647cγ−0.6772=0.
So,γ=−0.0729=−4.17∘. Then,
(44)gBI=0.99740.072700.1470−0.07270.99740−0.6772001001.
According to equation (11), there are
(45)gE1I=gBIgE1B=−0.69060.723200.2472−0.7232−0.690602.1798001001,gE2I=gBIgE2B=‐0.52830.84910‐1.9909‐0.8491‐0.528304.5258001001.Comparing the results of these situations, which are almost equal to each other, it can be concluded that the method is feasible and effective. The result of Situation 3 has a subtle error due to the truncation error of angles during the calculation.


## 4.1.2. Inverse Kinematical Solution of the Planar Model

Based on the method in Section3.2, iterative programs for each situation are written with MATLAB and are run separately. Firstly, the final pose of the two arms is given by
(46)gE1I=−0.69080.723100.2476−0.7231−0.690802.1798001001,gE2I=−0.52840.84900−1.9901−0.8490−0.528404.5261001001.The process of the iterative programs is as follow.Step 1.
Set the calculating parameters and known variables. Max number of iterations isN=100. Convergence tolerance is ε=0.001. Known parameters are Ψ=0,0,−4.18∘T (for Situation 2). pI0=0.1469,−0.6773,0T (for Situation 3).Step 2.
Set initial joint angles.θ0=60°,30°,80°,−60°,−30°,−80°T. For the program of Situation 3, the initial attitude angles of the base should also be given: Ψ0=0,0,−10°T.Step 3.
Calculate the Jacobian matrix of each iterative step using equation (23) (for Situation 2) or equation (30) (for Situation 3).Step 4.
Calculate the angles of each iterative step using equation (24) (for Situation 2) or equation (31) (for Situation 3).Step 5.
Calculate the residual error and judge whether the iteration is over. If yes, go to the next step. If no, go to step 2.Step 6.
Calculate the final pose of each arm using the iterative result of the angles.The results of the Adams simulation and the iterations are listed in Table1.Table 1
Simulation results of the Adams simulation and the iterations for the planar model.
Method
Joint angles (°)
Base’s attitude angles
Max error
Adams
83.15,18.94,128.4,−37.19,−29.44,−51.09T
0,0,−4.18°T
—
Iteration for Situation 2
83.096,19.014,128.351,−37.185,−29.449,−51.105T
—
0.39%
Iteration for Situation 3
83.089,19.020,128.364,−37.214,−29.443,−51.077T
0,0,−4.163°T
0.42%The comparison shows that the results of the iterations are quite approximate to the real solution, which demonstrates the accuracy of the proposed methods. Actually, comparison between forward kinematical solutions and results of the iterations could also verify the accuracy.Furthermore, efficiency of the methods is studied. Iterative time and number of iterative steps under certain calculating conditions are recorded while the initial parameters of the iteration of Situation 2 are changing. The results are presented in Table2.Table 2
Simulation results of iterative time and number of iterative steps.
Initial joint angles (°)
Iterative time (s)
Steps
100°,50°,60°,−80°,−60°,−100°T
5.94
11
60°,30°,80°,−60°,−30°,−80°T
3.41
6
65°,25°,95°,−55°,−30°,−60°T
3.51
6
70°,23°,105°,−45°,−30°,−55°T
2.97
5
75°,20°,115°,−40°,−30°,−50°T
2.47
4The results show that the iterative method is fairly efficient, and it could perform better when the initial parameters are closer to the real values. This is determined by the inherent property of Newton’s iterative method.


## 4.2. Solution of the Spatial Model

To verify the application of the proposed method to a tridimensional model, a calculating example of a single-arm three-link space robot is presented. The Adams model at the initial configuration and the final configuration is shown in Figures4 and 5, respectively.Figure 4
The Adams model of the single-arm three-link space robot at the initial configuration.Figure 5
The Adams model of the single-arm three-link space robot at the final configuration.The parameters of this spatial model are given as follows:(47)ω1=ω3=0,0,1T,ω2=0,−1,0Tr1=1.2030,0.6929,0T,r2=2.903,0.6929,0T,r3=4.603,0.6929,0TSo,(48)ξ1=ω1,r1×ω1T=0,0,1,0.6929,−1.2030,0T,ξ2=ω2,r2×ω2T=0,−1,0,0,0,−2.9030T,ξ3=ω3,r3×ω3T=0,0,1,0.6929,−4.603,0T,and m0=400,m1=15,m2=15,m3=24, and M=454.
(49)gC1B0=2.275E3×30.6929001,gC2B0=3.975E3×30.6929001,gC3B0=5.675E3×30.6929001,EBg0=6.306E3×30.6829001.


### 4.2.1. Forward Kinematical Solution of the Spatial Model

The final joint angles areθ=−67.51°,88.35°,21.62°T. According to equation (5), there are
(50)gC1B=RB1pB101=eθ1ξ^1gC1B0=0.38250.923901.6131−0.92390.38250−0.2976001001,gC2B=RB2pB201=eθ1ξ^1eθ2ξ^2gC2B0=0.01100.9239−0.38241.8651−0.02660.38250.9236−0.90630.999600.02881.071601,gC3B=RB3pB301=eθ1ξ^1eθ2ξ^2eθ3ξ^3gC3B0=0.35070.8549−0.38242.24790.11620.36540.9236−0.79850.9293−0.36830.02882.695501.According to equation (6), there is
(51)gEB=RBEkpBEk01=eθ1ξ^1eθ2ξ^2eθ3ξ^3gEB0=0.35070.8549−0.38242.46020.11620.36540.9236−0.72900.9293−0.36830.02883.285701.Situation 1.
Both the position and the orientation of the base are known:pI0=−0.3029,−0.0151,−0.0319T,Ψ=11.39°,27.60°,25.23°T.
So,(52)gBI=0.8017−0.33510.4950−0.30290.37770.92580.0149−0.0151−0.46330.17500.8688−0.031901.
According to equation (11), there is
(53)gEI=gBIgEB=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.Situation 2.
Only the orientation of the base is obtained:Ψ=11.39°,27.60°,25.23°T.
So,(54)RI0=0.8017−0.33510.49500.37770.92580.0149−0.46330.17500.8688.
According to equation (14), there is
(55)gEI=RI0RBERI0pBE−RI0M∑i=13mipBi01=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.Situation 3.
Only the position of the base is obtained:pI0=−0.3029,−0.0151,−0.0319T.
(56)pt=1Mm1pB1,+m2pB2+m3pB3=0.2338,−0.0820,0.1779T.
According to equation (16), there is
(57)cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ0.2338−0.08200.1779+−0.3029−0.0151−0.0319=0.
So,Ψ=δ,β,γT=11.385°,27.595°,25.236°T. Then,
(58)gBI=0.8017−0.33520.4949−0.30290.37780.92570.0150−0.0151−0.46320.17490.8688−0.031901.
According to equation (11), there is
(59)gEI=gBIgEB=0.70210.3805−0.60193.53990.25410.65580.71090.28910.6652−0.65200.36371.555601.


### 4.2.2. Inverse Kinematical Solution of the Spatial Model

Likewise, final pose of EE and the iteration parameters are given at first.(60)gEI=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.The max number of iterations isN=100. Convergence tolerance is ε=0.001. Known parameters are Ψ=11.39°,27.60°,25.23°T (for Situation 2). pI0=−0.3029,−0.0151,−0.0319T (for Situation 3).The initial joint angles areθ0=45°,45°,45°T and Ψ0=10,10,10∘T (for Situation 3).The results are listed in Table3. Accuracy of the proposed iterative methods is demonstrated while applying to the tridimensional model.Table 3
Simulation results of the Adams simulation and the iterations for the spatial model.
Method
Joint angles
Base’s attitude angles
Max error
Adams
−67.51°,88.35°,21.62°T
11.39°,27.60°,25.23°T
—
Iteration for Situation 2
−67.504°,88.332°,21.645°T
—
0.12%
Iteration for Situation 3
−67.263°,88.665°,21.874°T
11.590°,27.577°,25.065°T
1.73%


## 4.2.1. Forward Kinematical Solution of the Spatial Model

The final joint angles areθ=−67.51°,88.35°,21.62°T. According to equation (5), there are
(50)gC1B=RB1pB101=eθ1ξ^1gC1B0=0.38250.923901.6131−0.92390.38250−0.2976001001,gC2B=RB2pB201=eθ1ξ^1eθ2ξ^2gC2B0=0.01100.9239−0.38241.8651−0.02660.38250.9236−0.90630.999600.02881.071601,gC3B=RB3pB301=eθ1ξ^1eθ2ξ^2eθ3ξ^3gC3B0=0.35070.8549−0.38242.24790.11620.36540.9236−0.79850.9293−0.36830.02882.695501.According to equation (6), there is
(51)gEB=RBEkpBEk01=eθ1ξ^1eθ2ξ^2eθ3ξ^3gEB0=0.35070.8549−0.38242.46020.11620.36540.9236−0.72900.9293−0.36830.02883.285701.Situation 1.
Both the position and the orientation of the base are known:pI0=−0.3029,−0.0151,−0.0319T,Ψ=11.39°,27.60°,25.23°T.
So,(52)gBI=0.8017−0.33510.4950−0.30290.37770.92580.0149−0.0151−0.46330.17500.8688−0.031901.
According to equation (11), there is
(53)gEI=gBIgEB=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.Situation 2.
Only the orientation of the base is obtained:Ψ=11.39°,27.60°,25.23°T.
So,(54)RI0=0.8017−0.33510.49500.37770.92580.0149−0.46330.17500.8688.
According to equation (14), there is
(55)gEI=RI0RBERI0pBE−RI0M∑i=13mipBi01=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.Situation 3.
Only the position of the base is obtained:pI0=−0.3029,−0.0151,−0.0319T.
(56)pt=1Mm1pB1,+m2pB2+m3pB3=0.2338,−0.0820,0.1779T.
According to equation (16), there is
(57)cβcγsδsβcγ−cδsγcδsβcγ+sδsγcβsγsδsβsγ+cδcγcδsβsγ−sδcγ−sβsδcβcδcβ0.2338−0.08200.1779+−0.3029−0.0151−0.0319=0.
So,Ψ=δ,β,γT=11.385°,27.595°,25.236°T. Then,
(58)gBI=0.8017−0.33520.4949−0.30290.37780.92570.0150−0.0151−0.46320.17490.8688−0.031901.
According to equation (11), there is
(59)gEI=gBIgEB=0.70210.3805−0.60193.53990.25410.65580.71090.28910.6652−0.65200.36371.555601.


## 4.2.2. Inverse Kinematical Solution of the Spatial Model

Likewise, final pose of EE and the iteration parameters are given at first.(60)gEI=0.70220.3806−0.60183.54010.25390.65570.71100.28850.6652−0.65210.36381.555201.The max number of iterations isN=100. Convergence tolerance is ε=0.001. Known parameters are Ψ=11.39°,27.60°,25.23°T (for Situation 2). pI0=−0.3029,−0.0151,−0.0319T (for Situation 3).The initial joint angles areθ0=45°,45°,45°T and Ψ0=10,10,10∘T (for Situation 3).The results are listed in Table3. Accuracy of the proposed iterative methods is demonstrated while applying to the tridimensional model.Table 3
Simulation results of the Adams simulation and the iterations for the spatial model.
Method
Joint angles
Base’s attitude angles
Max error
Adams
−67.51°,88.35°,21.62°T
11.39°,27.60°,25.23°T
—
Iteration for Situation 2
−67.504°,88.332°,21.645°T
—
0.12%
Iteration for Situation 3
−67.263°,88.665°,21.874°T
11.590°,27.577°,25.065°T
1.73%


## 5. Conclusions

This paper mainly studied the kinematical problem, including forward and inverse kinematics, of the free-floating space-robot system at the position level. Kinematical equations are systematically established based on the screw theory. Three situations with different known variables are discussed in detail. In particular, a numerical method for the inverse kinematical solution is proposed by using Newton’s iteration method. Examples demonstrate the accuracy and efficiency of the developed method, which appears to indeed be concise and valid compared to the traditional D-H method. The proposed iterative method can rapidly calculate joint angles with sufficient accuracy; it could be quite useful when differential kinematics is not concerned.



---

*Source: 6857106-2019-06-02.xml*