Transcript 下載/瀏覽

FPGA-Realization of
Inverse Kinematics Control
IP for Articulated Robot
Advisor : Ying Shieh Kung
Student: Bui Thi Hai Linh
Mitsubishi robot arm
What is the Kinematic?
• Forward kinematics uses the
joint parameters to compute the
configuration of the chain.
• inverse kinematics reverses this
calculation to determine the
joint parameters that achieves a
desired configuration.
What is the Inverse Kinematics
• Inverse kinematics refers to the
use of the kinematics equations
of a robot to determine the joint
parameters that provide a
desired position of the endeffector
What is the Inverse Kinematics
• Kinematic analysis is one of the
first steps in the design of most
industrial robots. Kinematic
analysis allows the designer to
obtain information on the
position of each component
within the mechanical system
Analytical Inverse
Kinematics with Body
Posture Control
What is solution
What does the paper do?
• This paper studies and
implements the inverse
kinematics.
• Moreover, the FPGAimplementation of inverse
kinematics for two type’s robot
manipulators is presented.
What is robot
manipulator?
• A robot manipulator is composed of
a serial chain of rigid links
connected to each other revolute or
prismatic joints.
• A revolute joint rotates about a
motion axis
• A prismatic joint slide along a
motion axis.
a2
2
z1
x1
y1
a3
3
z2  4
x2
y3
y2
y4
d1
y5
1
z0
y0
x0
z3
x3
x4
z4
5
z5
d5
x5
R
1 0 0
0  1 0
TH  
0 0  1

0 0 0
x
y 
z

1
IK formulations
(2)
C1
S
0
A1   1
0

0
R
1
TH  0 A 5
C 2
S
A2   2
0

0
0  S1
0 C1
1 0
0
0
0
0 
d1 

1
nx
n
 0 A1 1 A2 2 A3 3 A4 4 A5   y
 nz

 S 2 0 a2C2 
0
C 2 0 a 2 S 2 
0
1
0 

0
0
1 
ox
oy
oz
ax
ay
az
0
0
px 
p y 
pz 

1
(4)
(5)
2
C3
S
A3   3
0

0
 S3
C3
0
0
0 a3 C 3 
0 a3 S 3 
1
0 

0
1 
(6)
C4
S
3
A4   4
0

0
0  S4
0 C4
1 0
0
0
0
0
0

1
(7)
The parameters of robot
manipulator‘s
Table 2.The parameters robot manipulator‘s
The Implementation in
FPGA
• The implementation of inverse
kinematics, we use parallel
processing method,
• it is resource consumption in
FPGA. The FPGA chip adopts
herein is an Altera DE2 Cyclone
II EP2C35F672
The Implementation in
FPGA
• The implementation of inverse kinematic is
developed by VHDL; therefore, the finite
state machine method is applied.
• In Fig.3, there are 42 steps to perform the
inverse kinematics in (34)~(40).
• The circuit needs 3 multipliers, 2 dividers,
2 adders, 1 square root function, 1
component for arctangent function, 1
component for arccosine function, 1 lookup-table for sine function and some
comparator for atan2 function.
Finite state machine
d1  d 2
(228)
Z
-
+
V1
x
A1
+
x
M2
x
-
Y2
X2
+A1
A1
+

A2
D2
a3
a2
(160)
(250)
V3
+
M3
1/r3
*3
x
V6
x
V22
V5
M3
V4
A1
3
cos-1
V2
M1

a 22  a 23
(88100) (80000)
M1
Y
X
2a 2a 3
V12
C2
LUT
for sin
(sin  3 )
t1
S1
Y/X
Y/X
D1
1 , 5
tan-1
atan2
C1
s0
s1
s2
s3
s4
Table II
s6
s5
s8
s7
s9
s15
s16
s17
s18
(a)
V2
1/r1
V5
1
x
M1
V1
V1
+
C2
x
M1
A1
V7
V5
x
a3
V2
(160)
V4
x
M3
s19
V7
x
M2
s20
1/r2
2
1/r5
5
x
*
5
+
S2
*2
M3
3
M3

-
x
x
M2
M1
*
1
S 2 / C2
D1
tan-1
atan2
2
-
+
1/r4
4
x
*
4
M1
C1
Table II
A1
s21
s22
s23
s24
s29
s25
(b)
s30
s38
s39
s40
s41
Simulation results in
Quartus II and Matlab
Implementation time
• The implementation of inverse
kinematics for robot manipulators
using FPGA has been successfully
demonstrated in this paper.
• The computation time to implement
the inverse kinematics in hardware
is only 840ns for articulated type
robot manipulator
• It was implemented by Nios II
processor in software need
5.60ms