AbstractThis paper features the kinematic modelling of a 5-axis arm having base, shoulder, elbow, tool pitch and tool roll and
stationary articulated robot arm which is used for doing successful consisting of only rotary joints [1]. The robot design consisted
robotic manipulation task in its workspace. To start with, a 5-axes of three parts, viz., mathematical modelling, mechanical
articulated robot was designed entirely from scratch and from
design, electronic design and the software design [14]. There
indigenous components and a brief kinematic modelling was
performed and using this kinematic model, the pick and place task are 5 joints, 5 axis (3 major axes - base, shoulder elbow : to
was performed successfully in the work space of the robot. A user position the wrist and 2 minor axis - pitch and roll : to orient
friendly GUI was developed in C++ language which was used to the gripper in the direction of the object). Since n = 5 ; 20
perform the successful robotic manipulation task using the developed kinematic parameters are to be obtained and 6 unit frames are
mathematical kinematic model. This developed kinematic model also to be attached to the various joints [2] as shown in the link
incorporates the obstacle avoiding algorithms also during the pick and
coordinate diagram in Fig. 2.
place operation.
KeywordsRobot, Sensors, Kinematics, Computer, Control,
PNP, LCD, Software.
I. INTRODUCTION
363
World Academy of Science, Engineering and Technology 4 2007
a3 a4 has its own set of sprocket and chains which then determine
2
x
3 3 4 the joint angle precision [15]. There are three links a2 and
L2 2 z L4
z a3 and a4. The height of the shoulder from the base is d1
4 5 and the tool / gripper length is d5. d1 is the height of the
3 Elb y2 TP x3 TR x4 shoulder from the base which can be seen in Fig. 4.
y4
y3 z4
a2 III. DIRECT KINEMATIC ANALYSIS ALGORITHM & THE
2 L3 5 d5 KINEMATIC MOEL
1 5
z x
Given the joint variable vector q ( for rotary joint) and
2 1
Tool
x the Geometrical Link Parameters ( GLP - physical dimensions
Sh p
L1 y5 of the robot arm : constant for a given robot ), finding the
1 L5
y position p of the tip of the gripper and the orientation R of
5
z the gripper of the robot arm w.r.t. base of the robot from
d1 1 the reference position is called as direct kinematics as
0 shown in Fig. 3. To solve the DKP means to find the p
z 0
y and R of the tool w.r.t. base [1].
L0
5
x0 JS R
1 CHCTM
Given sets of Arm matrix
B joint variables DKP
5
q = { q 1 , q 2 , q 3 , q4 , q5 }
T p,R T0
Fig. 2 Link coordinate diagram of the robot arm [1]
T
= { 1 , 2 , 3 , 4 , 5 } ?
The designed robot is a educational five axis articulated = ? To be found out
table-top robot entirely designed and fabricated and is having GLP
a1 to a5 , d1 to d5 ,
a non-spherical wrist, i.e., the pitch and the roll axes meets at
1 to 5
different points. The five axes are : five DOF - Base,
( given )
Shoulder, Elbow, Pitch, Roll and no Tool Yaw. Base motor
is mounted vertically on a horizontal plane. Shoulder, Fig. 3 Direct kinematic input-output model of the designed robot
elbow, tool pitch motors are mounted horizontally on the arm
base [16]. The tool roll and grip motors are mounted at To find the position and orientation of the robot arm
the wrist joint and are very small. Base axis is fixed and means, we have to find a matrix called as the arm matrix,
is vertical, while the shoulder, elbow and tool pitch axes
are horizontal and rotates about the base axes, i.e., the i.e., the composite homogeneous coordinate transformation
base and shoulder are perpendicular, shoulder and elbow matrix, which is a (4 4) matrix [19]. How does this
are parallel, elbow and pitch are parallel, while the pitch matrix give the position and orientation of the robot w.r.t.
and roll are perpendicular [17]. All the joints are rotary / base from the reference position ? The 1st three columns
revolute / articulated in nature, q = is the joint variable. gives the three possible orientations ( Yaw, Pitch, Roll ) of
Our designed robot is computer controlled and electrically the gripper and the last column gives the position of the
driven and is shown in the one line diagram in Fig. 2. It tip of the gripper p , thus solving the DK problem. If we
uses D.C. servos and incremental encoders and open loop give this matrix as input to the robot, the robot will go and stop
control / closed loop control, electrically driven, uses PTP
in that particular position and in that particular orientation [1].
control and the load shaft precision is very good. Power
is transmitted to the shoulder, elbow and tool pitch using
TABLE I KINEMATIC PARAMETER TABLE OF THE DEVELOPED ROBOT
gears and chains. The power transmission devices are the
chains and gears [7]. Axis Type k dk ak k SHP
The designed and fabricated robot is used for illustrating the
theoretical concepts and practical concepts relating to a 1 Base 1 d1 0 /2 0
robot and to perform some laboratory experiments. Each 2 Shoulder 2 0 a2 0 /2
axis is driven by a DC servomotor (with built in gears)
that has a incremental encoder attached to the high
3 Elbow 3 0 a3 0 /2
speed shaft. The encoders resolve the high speed position 4 Tool pitch 4 0 a4 /2 0
to 60. Since , each motor has a built in gear head with a
5 Tool roll 5 d5 0 0 /2
turns ratio of 66.1 : 1 or 96 : 1, this results in a precision
for each load shaft of 0.624 / count [18].
There are 20 kinematic parameters and 6 RHOCFs in the A. Direct kinematic algorithm :
Link Coordinate Diagram (LCD) shown in Fig. 2. Each joint Draw the SLD of the designed robot with links represented
by straight lines ; joints by small circles called as nodes [8].
364
World Academy of Science, Engineering and Technology 4 2007
T 50 = T 30 T 53 (4)
Using 1 pass of DH algorithm, assign (5 + 1) = 6 right
st
Wrist (Pitch)
handed orthonormal coordinates L0 to base, L1 to shoulder, T Tool-tip
Base = T Base T GWripper-tip
rist (P itch) (5)
L2 to the elbow, L3 to the tool pitch, L4 to tool roll, L5
to the tip of the gripper, p as shown in the Fig. 2 [20]. 1 2
major axes minor axes
f 3 4 f f
1 3 4 5
Using 2nd pass of the DH algorithm, find the (4 5) = 20
2
5
KPs and obtain the kinematic parameter table KPT as
shown in the table 1 [21].
C. Computation of the First Wrist Partitioned Arm Matrix
Put k = 1 to 5 and the different rows of the KPT in the T03 = T01 T12 T23 (6)
general link coordinate transformation matrix T kk 1 and C1 0 -S1 0 C2 -S2 0 a 2 C2 C3 -S3 0 a 3 C3
obtain the various fundamental homogeneous coordinate
= S1 0 C1 0 S2 C2 0 a 2 S2 S3 C3 0 a 3 S3
transformation matrices T 10 , T 12 , T 32 , T 34 , T 54 . 0 -1 0 d1 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
Since n > 4, partition the arm matrix T 50 at the wrist so
C1C2 C1S2 S1 a1 C1 C2 C3 S3 0 a 3 C3
that we get two wrist partitioned matrices [22]. One
which gives the position and orientation of the wrist S1C2 S1S2 C1 a1 S1 S2 S3 C3 0 a 3 S3
=
w.r.t. the base, i.e., T 30 and the other which gives the S C 2 0 d1 a 2 S2 0 0 1 0
2
position and orientation of the gripper w.r.t. the wrist , 0 0 0 0 1
0 0 1
i.e., T 53 .
C1( C2C3 S2S3 ) C1( S2C3 + C2S3 ) S1 C1 {a 3 ( C2C3 - S2S3 ) + a 2C2 }
= S ( C C S S ) S ( S C + C S ) C S1{a3 ( C2C3 - S2S3 ) + a 2C2 }
Multiply the first three fundamental HCTMs T 10 , T 12 , 1 2 3 2 3 1 2 3 2 3 1
( S2C3 + C2S3 ) ( C2C3 + S2S3 ) 1 d1 a 2S2 a 3 S2C3 + C2S3 ( )
T 32 . Obtain the arm matrix T 30 = T 10 T 12 T 32 0 0 0 1
Multiply the next two fundamental HCTMs T 34 , T 54 . C1 C23 C1 S23 S1 C1 ( a 2 C2 +a 3 C23 )
5 4 5
S C S1 S23 C1 S1 ( a 2 C2 +a 3 C23 )
T03 = 1 23
Obtain the arm matrix T = T 3 3 T 4
(7)
Multiply the two wrist partitioned matrices, T 3
and T 5 S23 C23 0 d1 a 2 S2 a 3 S23
0 3
to obtain the output of direct kinematic problem, i.e., T 50 . 0 0 0 1
This matrix T03 gives position and orientation of the
Substitute the soft home position - SHP angles (last column wrist (pitch) coordinate frame L3 w.r.t. the base frame L0.
of KPT) in the computed arm matrix T 50 and compute T 50
To check this whether the matrix obtained is correct or
in the home position. Verify the LCD & get the arm not, evaluate it at the Soft Home Position [ SHP ] by
equations which are very useful in the kinematic putting the values of the angles given in the last column
modelling [1], [23].
of KP table in T03 ;
i.e., put q = [ 0 , 90 , 90 ]T = {q1 , q2 , q3 }T in the
B. Arm Matrix
computed T03 matrix, we get ;
The arm matrix is divided into three parts, viz., first
partitioned matrix T 30 , second partitioned matrix T 53 and 1 0 0 a3
5
the final arm matrix T [9], [24]. 0 0 1 0
T03 (home) =
0
0 1 0 d1 +a 2
To find the position p and orientation R of gripper w.r.t.
base , use successive HCTM s starting from the tip of 0 0 0 1
the gripper and ending at the base [1], [25]
Tool
TBase Shoul
(q) = TBase Elbow
TShould Pitch
TElbow Roll
TPitch Tip
TRoll (1) R11 R12 R13 p1
R R 22 R 23 p 2
T03 = 21 (8)
T 50 = T 10 T 12 T 32 T 34 T 54 . (2) R 31 R 32 R 33 p3
T 50 = T01 T12 T23 T34 T45 (3) 0 0 0 1
365
World Academy of Science, Engineering and Technology 4 2007
T35 = 4
S 0 C4 a 4 C4 5 C5 C5S234 S234S5 C234
0 1 0 0 0 0 1 d5
0 0 0
0 0 0 1 0 0 0 1
C1 ( a 2 C2 +a 3C23 +a 4 C234 d 5S234 )
C 4 C5 C 4 S5 S4
a 4 C 4 d 5 S4
S C S1 ( a 2 C2 +a 3C23 +a 4 C234 d 5S234 )
S4 S5 C4 a 4S4 +d 5 C 4
= 4 5 (10) d1 a 2S2 a 3S23 a 4S234 d 5S234
S5 C5 0 0
1
0 0 0 1
5
This matrix T3 gives the position and orientation of the
R11 R12 R13 p1
R R 22 R 23 p 2
tip coordinate frame L5 w.r.t. the wrist coordinate frame L3. T05 = 21 (12)
To check this whether the matrix obtained is correct or R 31 R 32 R 33 p3
not, evaluate it at the Soft Home Position [ SHP ] by
putting the values of the angles given in the last column 0 0 0 1
of KP table in T35 , i.e., put q = { 0 , 90}T = {q4 , q5 }T To check this matrix whether it is correct or not,
evaluate it at SHP, by putting the SHP angles which are
in the computed T35 , we get [11], [26];
given in the last column of the KPT in this computed
0 1 0 a3 final arm matrix, T05 , i.e., q = { q1 , q2 , q3 , q4 , q5}T = { 1 ,
0 0 1 d 5
T35 (home) = 2 , 3 , 4 , 5}T = { 0, 90, 90 , 0 , 90 }T in T05 . Check
1 0 0 0
that the norms of the rotation matrix of T05 . They are all
0 0 0 1
unity. This arm matrix T05 given by Eqn (12) is the output
of direct kinematics of the designed five axes articulated robot
R11 R12 R13 p1
arm, thus giving the position and orientation of the gripper
R R 22 R 23 p 2 w.r.t. base . The 1st three columns gives the orientation of
T35 = 21 (11)
R 31 R 32 R 33 p3 the frame L5 w.r.t. base, while the last column gives the
position of the tip of the gripper p w.r.t. base, thus obtaining
0 0 0 1
a unique direct kinematic model of the designed robot [1], [27].
Note that this is coincident from the LCD. Thus, the
LCD is also verified.
x5 y5 z5 p
E. Computation of the Final Arm Matrix / CHCTM, T05 x 0 0 1 0 a3 + a4
0
To compute the final arm matrix ; multiply the two
wrist partitioned matrices T03 and T35 given by Eqs. (7) and T05 (SHP) = y 1 0 0 0
(10). Simplify the arm matrix by using some assumptions as z0 0 0 1 d1 +a 2 d 5
T05 = T03 T35 0 0 0 1
C1 C23 C1 S23 S1 C1 ( a 2 C2 +a 3 C23 )
S1 C23 S1 S23 C1 S1 ( a 2 C2 +a 3 C 23 )
=
S23 C23 0 d1 a 2 S2 a 3 S23
0 0 0 1 Obtain the arm equations by equating T05 = T05 (SHP).
R11 = C1C234C5 + S1S5 = 0
R21 = S1C234C5 C1S5 = 1
R31 = C5S234 = 0
366
World Academy of Science, Engineering and Technology 4 2007
367
World Academy of Science, Engineering and Technology 4 2007
VII. CONCLUSIONS
A unique 5-axes articulated system was used to obtain the
kinematic model of the same and was used to perform a
successful pick and place task using a user-friendly developed
graphical user interface and real time implementation. The
simulated results were exactly verified with implementation
results, thus demonstrating a effective PNP manipulation.
368
World Academy of Science, Engineering and Technology 4 2007
369