Research Journal of Applied Sciences, Engineering and Technology 4(16): 2645-2648, 2012
ISSN: 2040-7467
© Maxwell Scientific Organization, 2012
Submitted: December 12, 2011
Accepted: January 13, 2012
Published: August 15, 2012
An Algorithm of Inverse Kinematics for the Automated Fiber
Placement Robotic Manipulator
1,2
G.E. Xin-Feng, 1Z.H.A.O. Dong-Biao, 1L.U. Yonghua and 1L.I.U. Kai
College of Mechanical, Electrical Engineering, Nanjing University of Aeronautics and
Astronautics, Nanjing 210016, China
2
College of Electrical and Information Engineering, Xuchang University, Xuhang 461000, China
1
Abstract: To solve inverse kinematics of the automated fiber placement robotic manipulator, an algorithm
based on the position vector and posture transformation matrix is proposed. According to the structural
characteristics of three revolute joint axes of the automated fiber placement robotic manipulator intersect at one
point, three displacement joint variables and three revolute joint variables are calculated, respectively using the
position vector and posture transformation matrix. Compared with the general iterative algorithm, the algorithm
proposed in this paper reduces the number of solving inverse matrices, increases solving speed and is expressed
more simply. The algorithm is verified by simulation and the simulation result shows that the proposed
algorithm in this study is correct.
Keywords: Inverse kinematics algorithm, position vector, posture transformation matrix, robotic manipulator
INTRODUCTION
Fiber placement technology is a new composite
material manufacturing technology. In recent years, fiber
placement technology has developed quickly and has been
widely used in developing the aircraft parts of F22, F35,
V22 and A380 (Li and Xiao, 2002). Fiber placement
technology research in the domestic field has been a
breakthrough generally, but mostly for materials and
related software (Zhang and Sarhadi, 1996). Research for
fiber placement equipment has just started, but the
research for fiber placement equipment to complex shape
parts is rarely reported. The purpose of researching and
developing fiber placement robotic manipulators is to
supply manufacturing tools for complex shape parts.
According to fiber placement path planning, the tasks of
the fiber placement robotic manipulator are to arrive at the
designated location in a variable posture and complete the
fiber placement. Kinematics is the basis in researching the
robot and inverse kinematics is particularly important for
fiber placement robotic manipulators.
At present, algebra (Serdar and Zafer, 2004),
geometry and numerical algorithms are being used to
solve the inverse kinematics. The numerical method is
divided into direct and indirect methods; the Newton
method and the Newton-Lipson method (Huo, 2009)
belong to the direct method. If the Jacobian matrix is
singular, there are no feasible solutions and if the initial
Corresponding Author:
position is not sufficiently close to the target location, the
problems are unsolvable in the direct method. The indirect
method is based on the optimized method and the methods
include the CCD method, BFS method, CCD and BFS
method and genetic algorithms. However, these
algorithms are relatively complex and are not suitable for
real-time industry control.
According to the specific structure of the automated
fiber placement robotic manipulator, three displacement
joint variables and three revolute joint variables are
calculated respectively using the position vector and
posture transformation matrix. Compared with the general
iterative algorithm, the algorithm proposed in this study
reduces the number of solving inverse matrices, increases
the solving speed and is expressed more simply. The
algorithm was verified by simulation and the simulation
results show that the proposed algorithm in this study is
correct.
TOPOLOGY AND PARAMETERS
The automated fiber placement robotic manipulator
is an open space linkage chain, formed through three
displacement joints and three revolute joints in a series.
The three revolute joint axes intersect at one point, with
one end fixed at the base while the other is free. The
free end has a tool installed that is used to place the fiber,
called an end-effector, and relative to the end-effector
G.E. Xin-Feng, College of Mechanical, Electrical Engineering, Nanjing University of Aeronautics,
Astronautics, Nanjing 210016, China
2645
Res. J. Appl. Sci. Eng. Technol., 4(16): 2645-2648, 2012
challenging than the direct kinematics problem (Manfred
and Martin, 2007). Assuming the posture transformation
matrix of the robotic manipulator end-effector coordinate
system {n} relative to the base coordinate System {0} is:
T = 01T (θ1 ) 21T (θ2 )...n −n1T (θn )
0
n
(1)
Known as the manipulator end-effector kinematics
equation, this shows the relationship between the endeffector pose and the joint variables.
Fig.1: The automated fiber placement robotic manipulator’s
structure
Table 1: The automated fiber placement robotic manipulator’s D-H
parameters
"i-1
"i- 1
di
2i
The scope of the
linki
(mm)
(º)
(mm)
(º)
joint Variables
0
d1:!150!150 mm
1
0
0
d1
2
0
90
d2
-90
d2:!110!110 mm
3
"2
90
d3
0
d3:!100!100 mm
4
0
0
c
24
24:!210º!210º
5
0
90
0
25
25:!150º!150º
6
0
-90
0
26
26:!260º!260º
is a mandrel that can rotate. The structure is shown in Fig.
1. The automated fiber placement robotic manipulator’s
movement is synthesized by the mandrel’s rotation and
the robotic manipulator’s translation and rotation. The
robotic manipulator end-effector is always moving on the
mandrel’s surface in a working process, so the robotic
manipulator end-effector’s motion is actually the
movement on the mandrel surface along a specific
trajectory. In order to analyze the kinematics, the robotic
manipulator link coordinate systems should be
established, the relationship between the coordinate
systems should be studied and the kinematics equations
should be established by using D-H parameters. The D-H
parameters are shown in Table 1.
DIRECT AND INVERSE KINEMATICS
PROBLEM
The kinematics analysis of serial robotic manipulators
comprises direct and inverse kinematics problems. The
direct kinematics problem is the mapping from a joint
space to an operational space. For serial manipulators, this
problem is straightforward and admits a unique solution,
which can be determined by simple matrix and vector
multiplications. Alternatively, the problem with inverse
kinematics is the mapping from an operational space to a
joint space (Sciavicco and Siciliano, 2000). In general, the
inverse kinematics problem is much more complex and
The direct kinematics problem: The direct kinematics
problem is the mapping from a joint space to an
operational space.
Assuming the position and posture transformation
matrix of the manipulator end-effector coordinate system
relative to the base coordinate system is:
⎡ 60 n x
⎢0
0
0 1 2 3 4 5
⎢ 6 ny
6T = 1T 2T 3T 4T 5T 6T =
⎢ 60 nz
⎢
⎣ 0
0
6
0
6
0
6
ox
oy
oz
0
0
6
0
6
0
6
ax
ay
az
0
0
6
0
6
0
6
px ⎤
⎥
py ⎥
(2)
pz ⎥
⎥
1 ⎦
According to (2), the value of each element in 06T can be
obtained using Mat lab:
⎧ 0n = − sin θ cosθ
5
6
⎪6 x
⎪ 60n y = − sinθ4 cosθ5 cosθ6 − cosθ4 sin θ6
⎪
⎪ 0n = sin θ sin θ − cosθ cosθ cosθ
4
6
4
5
6
⎪6 z
⎪ 60nx = sinθ5 sin θ6
⎪⎪
0
⎨ 6 o y = sin θ4 cosθ5 sinθ6 − cosθ4 cosθ6
⎪0
⎪ 6 oz = cosθ4 ,cosθ5 sinθ6 + sin θ4 cosθ6
⎪0
0
⎪ 6 a x = − cosθ5 ,6 α y = sin θ4 cosθ5
⎪0
0
⎪ 6 az = cosθ4 sinθ5 ,6 px = − c − d3
⎪0
0
⎪⎩ 6 py = − d2 ,6 pz = d1 − a2
(3)
From (3) it can be seen that the end effector coordinate
origin’s coordinate (px, py, pz) in the basis coordinate
system is entirely determined by three displacement joint
variables d1, d2, d3 and the end effector coordinate system
posture (nx, ox, ax)( ny, oy, ay) (nz, oz, az ) relative to the
basis coordinate system is entirely determined by three
revolute joint variables 24, 25, 26. So displacement joints
are usually called manipulator location joints and revolute
joints are usually called manipulator posture joints.
The inverse kinematics problem: The inverse
kinematics problem is the inverse process of the direct
2646
Res. J. Appl. Sci. Eng. Technol., 4(16): 2645-2648, 2012
kinematics problem; it pertains to the mapping from an
operational space to a joint space. In general, the inverse
kinematics problem is much more complex and
challenging than the direct kinematics problems and the
solutions may be many, or there may be none, but they
generally do not have a closed form (Liu et al., 1995).
According to the structural characteristics of the three
revolute joint axes of the automated fiber placement
robotic manipulator intersect at one point, three
displacement joint variables and three revolute
jointvariables are calculated respectively using the
position vector and posture transformation matrix. The
relationship of the posture transformation matrix shows
the position vector of the end-effector coordinate system
relative to the base coordinate system:
0
P6 = 0T11T2 2T3 3 P6
0 ⎤⎡ 0
1 0 0 ⎥⎥ ⎢⎢ 0
0 1 d1 ⎥ ⎢ − 1
⎥⎢
0 0 1 ⎦⎣ 0
⎡1
⎢0
= ⎢
⎢0
⎢
⎣0
0 0
0 ⎤ ⎡1
0 − 1 − d2 ⎥⎥ ⎢⎢ 0
0 0
0 ⎥ ⎢0
⎥⎢
0 0
1 ⎦ ⎣0
1
0
a2 ⎤
0 − 1 − d3 ⎥⎥
1 0
0 ⎥
⎥
0 0
1 ⎦
0
⎡ − d ⋅ a x + px − X off ⎤
⎥
⎢
⎢ d ⋅ a x − px − Zoff ⎥
0
P6 = ⎢
⎥
− d ⋅ a y + py
⎥
⎢
⎥⎦
⎢⎣
1
The displacement joint variables can be obtained from (6)
(7):
⎧ d1 = p y − d ⋅ a y + a 2
⎪
⎨ d 2 = Z off + pz − d ⋅ a z
⎪
⎪⎩ d 3 = d ⋅ a x + X off − c − p x
(4)
(T10T21T32 ) −1 T60 = T43T54 T65
⎡ cosθ4
⎢ sinθ
4
T43T54T65 ⎢
⎢ 0
⎢
⎣ 0
ox
ax
oy
oz
ay
az
0
0
px ⎤
p y ⎥⎥
pz ⎥
⎥
1⎦
(5)
T = S0T −1 TS T T6T −1
0 − X off ⎤ ⎡ n x o x a x p x ⎤ ⎡ 1
0 − 1 − Z off ⎥ ⎢ n y o y a y p y ⎥ ⎢ 0
⎥⎢
⎥⎢
1 0
0 ⎥ ⎢ n z oz a z p z ⎥ ⎢ 0
⎥⎢
⎥⎢
0 0
1 ⎦⎣ 0 0 0
1 ⎦ ⎣0
⎡ n x o x a x − d ⋅ a x + p x − X off ⎤
⎢n o a
d ⋅ a z − pz − Z off ⎥
z
z
z
⎥
= ⎢
⎢ ny oy a y
⎥
− d ⋅ a y + py
⎢
⎥
1
⎣0 0 0
⎦
0
− sin θ4
cosθ4
0
0
0 0⎤ ⎡ cosθ5 − sin θ5 0
−1
0 0⎥⎥ ⎢⎢ 0
0
1 c ⎥ ⎢ sin θ5 cosθ5
0
⎥⎢
0 1⎦ ⎣ 0
0
0
⎡ cosθ6 − sinθ6 0 0⎤ ⎡ cos 4 cos5 cos 6 − sin 4 sin 6
⎢ 0
0
1 0⎥⎥ ⎢⎢ sin 4 cos 5cos 6 + cos 4 sin 6
⎢
⎢ − sin θ6 − cosθ6 0 0⎥ ⎢
sin 5cos 6
⎥⎢
⎢
0
0
0
1
0
⎦⎣
⎣
− cos 4 cos5 sin 6 − sin 4 cos 6 − cos 4 sin 5 0⎤
− sin 4 cos5 sin 6 + cos 4 cos 6 − sin 4 sin 5 0⎥⎥
− sin 2 cos 6
cos5
c⎥
⎥
0
0
1⎦
Thus,
⎡1
⎢0
= ⎢
⎢0
⎢
⎣0
(9)
and,
Assuming the posture transformation matrix STT of
the end-effector coordinate system T relative to the
module coordinate system S is:
0
6
(8)
From T06 = T01T12T23T34T45T56 we know:
0
⎡ 0⎤ ⎡ − c − d3 ⎤
⎢ 0⎥ ⎢ − d ⎥
2 ⎥
= ⎢ ⎥⎢
⎢ c ⎥ ⎢ d1 − d2 ⎥
⎥
⎢ ⎥⎢
⎦
⎣1 ⎦ ⎣ 1
⎡ nx
⎢n
S
S 0 6
⎢ y
T T = 0T 6T TT = ⎢
nz
⎢
⎣0
(7)
0⎤
0⎥⎥
0⎥
⎥
1⎦
(10)
Assuming,
0 ⎤
0 ⎥
⎥ (6)
0 1 − d⎥
⎥
0 0 1 ⎦
0 0
1 0
⎡ T11 T12
⎢T
21 T22
(T10T21T32 ) −1 T60 = ⎢
⎢ T31 T32
⎢
0
⎣ 0
T13 0⎤
T23 0⎥⎥
T33 0⎥
⎥
0 1⎦
(11)
The revolute joint variables can be obtained from (10)
(11):
25 = arcos (T33) = arcos(-"x)
From (6) we know:
2647
(12)
Res. J. Appl. Sci. Eng. Technol., 4(16): 2645-2648, 2012
simulation trajectory is in Fig. 3 according to the desired
trajectory, using the obtained inverse kinematics.
By comparing Fig. 2 and 3 we can see that the
simulation trajectory and the desired trajectory are in good
agreement and it illustrates that the kinematics equations,
the direct kinematics and the inverse kinematics
algorithms in the paper are all correct.
CONCLUSION
The kinematics equations are established and the
direct kinematics and the inverse kinematics are solved
using D-H parameters according to the simplified model
of the automated fiber placement robotic manipulator. An
inverse kinematics algorithm is presented suitable for the
automated fiber placement manipulator and the computed
speed is increased. The simulation was conducted using
the established kinematics equations and the desired
trajectory and the results show that the proposed
kinematics equations and the inverse kinematics algorithm
are correct.
Fig. 2: The desired trajectory of the end-effector
ACKNOWLEDGMENT
This study is financially supported by National
Natural Science Foundation of China (51175261;
51005122), Air Fund (2008ZE52049), to express my
gratitude.
Fig. 3: The simulation trajectory of the end-effector
If 25 = 0, the robotic manipulator is in a singular
configuration, 24 and 26 can not be obtained; if 25…0, 24
and 26 can be obtained:
⎧ θ4 = arctan (T23 / T13 ) = arctan ( − a z / a y )
(13)
⎨
⎩ θ6 = arctan ( − T32 / T31 ) = arctan ( − o x / n x )
It should be noted that there may be many solutions
to inverse kinematics problems. However, some solutions
can not be achieved due to structural limits, i.e., revolute
joints can not rotate within the range of 360 degrees.
Therefore, in the case of many solutions, the most
appropriate solution should be selected to meet the study
requirements.
KINEMATICS SIMULATIONS
The aim of kinematics simulation is to verify whether
the kinematics equations, the direct kinematics and the
inverse kinematics algorithms are correct or not and then
to establish the foundation for the dynamics. The endeffector desired trajectory of the automated fiber
placement manipulator is in Fig. 2 and the end-effector
REFERENCES
Huo, L., 2009. Robotic Joint-Motion Optimization of
Functionally-Redundant Tasks for Joint-Limits and
Singularity Avoidance. Montréal: University of
Montréal, Canada.
Li, Y. and J. Xiao, 2002. The technology and application
of fiber placement. Fiber Composites, 3: 39-41.
Liu, L., Y. Wang and Q. Zhang, 1995. On the numerical
solutions of inverse kinematics. J. Beijing Univ.
Aeronaut. Astronautics, 1: 120-125.
Manfred, L.H. and P. Martin, 2007. A new and efficient
algorithm for the inverse kinematics of a general
serial 6R manipulator. Mech. Mach. Theory,
42: 66-81.
Sciavicco, L. and B. Siciliano, 2000. Modeling and
control of robot manipulators. Springer, 3: 377-381.
Serdar, K. and B. Zafer, 2004. The inverse kinematics
solutions of industrial robot manipulators.
Proceedings of the IEEE International Conference on
Mecha-tronics.
Zhang, Z. and M. Sarhadi, 1996. An integrated
CAD/CAM system for automated composite
manufacture. J. Mater. Process. Tech., 61(1-2):
104-109.
2648