Forward Kinematics for 3R Manipulator using DH method

avatar

1. Introduction

In this blog, we model the forward kinematics of a robotic arm with three links using Denavit-Hartenberg representation. We are given the three link lengths and their respective angles. The model returns the location of the end-effector. Forward kinematics is constituted of successive connections which are attached to each other revolute or prismatic joints from the base frame through the end-effector. A suitable kinematics model should be used in order to have a methodical manner with respect to the robot mechanism. The combined space reveals small data with regards to the position and orientation of the end-effector of the kinematic chain.

Forward kinematics describes a mapping from the combined connection space to the three-dimensional Cartesian space. Given the kinematic chain m joints and a set of values (θ1,θ2,...,θm), it can seek for the position (px,py,pz) and the orientation (ax,ay,az) of the end-effector of the kinematic chain in the three-dimensional x-y-z space.

Furthermore, forward kinematics can be solved for any simple or complicated kinematic chain which would revenue an adjacent and logical explanation because it is a domain-independent problem. In the next section, the mathematical model is presented and simulated in MATLAB.

2. Mathematical Model

The given parameters in forward kinematics are L1,L2,L3,θ1,θ2 and θ3 then we are required to solve for the location and coordinates of the end-effector (x,y) using Denavit-Hartenberg convention. The end-effector (x,y) is any device at the end of robotic arm.

image.png

The Denavit-Hartenberg follows these three basic rules for determining and establishing each coordinate frame. The rules are:

  1. The Z_(i-1)axis lies along the axis of motion of the ith joint.
  2. The X_i axis is normal to the Z_(i-1)axis and pointing away from it.
  3. The Y_i axis completes the right-handed coordinate system as required.

By these rules, one is free to choose the location of coordinate frame 0 anywhere in the supporting base, as long as the Z_0 axis lies along the axis of motion of the first joint. The last coordinate (nth frame) can be placed anywhere in the hand as long as the X_n axis is normal to the Z_(n-1) axis. The D-H representation of a rigid link depends on four geometric parameters associated with each link. These four parameters completely described any revolute or prismatic joint. These four parameters are defined as follows:

  1. θi is the joint angle from the X(i-1) to the X_i axis about the Z_(i-1) axis (using the right-hand rule).
  2. d_i is the distance from the origin of the (i-1)th coordinate frame to the intersection of the Z_(i-1) axis with the X_i axis along the Z_(i-1) axis.
  3. a_i is the offset distance from the intersection of Z_(i-1) axis with the X_i axis to the origin of the ith frame along the X_i axis (or the shortest distance between the Z_(i-1) and Z_i axes.
  4. αi is the offset angle from the Z(i-1) axis to the Z_i axis about the X_i axis (using the right hand rule).

In this convention, each homogeneous transformation A_i is represented as a product of four basic transformations.

qw.png
qw2.png

The upper left 3x3 sub matrix represents the rotation matrix. The upper 3x1 sub matrix represents the position vector of the origin of the rotated coordinate system with respect to the reference system. The lower left 1x3 sub matrix represents the perspective transformation and the lower right 1x1 sub matrix represents the global scaling.

qw3.png
Figure 1. Three-link robotic arm (3 degrees of freedom)

The listed values of the table above were derived from the Denavit-Hartenberg representation using the rules of the four parameters listed above. The values of α_1 to α_3,a_1 to a_3 and d_1 to d_3 will be used in MATLAB simulation except for θ_i that will be set by the user.

3. MATLAB implementation

In this section we will implement the derive values of α_i,a_i,d_i of a three-linked robotic arm of Forward Kinematics using Denavit-Hartenberg convention in MATLAB. We are required to solve for the location and coordinates of the end-effector (x,y) given the lengths of the three links of a robotic arm L_1, L_2 and L_3 and their corresponding angles θ_1, θ_2 and θ_3.

By inputting the values of L_1=3, L_2=1.5, L_3=1 θ_1=45, θ_2=45 and θ_3=30 will give the location and coordinates of end-effector (x,y) and the corresponding plot of the given links and their respective angles. The user will input the length of each links and their respective angles as shown in Figure 2. Figure 3 shows the plots of each link, joint and end effector.

image.png
Figure 2. Needed inputs of each length and their corresponding angles in the command window of MATLAB

image.png

Figure 3. Plot of each link, joint and end-effector

MATLAB Script

clc;
clear all;
close all;
 
% Needed Inputs
Link1=input('Input the length of the first link: ');
Link2=input('Input the length of the second link: ');
Link3=input('Input the length of the third link: ');
theta1 = input('Input theta 1 in degrees:');
theta2 = input('Input theta 2 in degrees:');
theta3 = input('Input theta 3 in degrees:');
 
% DENAVIT-HARTENBERG Parameters
alpha1 = 0; % offset angle
alpha2 = 0;
alpha3 = 0;
d1 = 0; % distance from the origin of the (i-1)th coordinate frame to the 
% intersection of the Zi(i-1) axis with the Xi axis along the Zi(i-1) axis.
d2 = 0;
d3 = 0;
 
ja11 = cosd(theta1); 
ja12 = -cosd(alpha1)*sind(theta1); 
ja13 = sind(alpha1)*sind(theta1);
ja14 = Link1*cosd(theta1); 
ja21 = sind(theta1); 
ja22 = cosd(alpha1)*cosd(theta1); 
ja23 = -sind(alpha1)*cosd(theta1);
ja24 = Link1*sind(theta1); 
ja31 = 0;
ja32 = sind(alpha1);
ja33 = cosd(alpha1);
ja34 = d1;
 
T1 = [ja11 ja12 ja13 ja14; ja21 ja22 ja23 ja24; ja31 ja32 ja33 ja34; 0 0 0 1];
 
jb11 = cosd(theta2); 
jb12 = -sind(theta2); 
jb13 = sind(alpha2)*sind(theta2);
jb14 = Link2*cosd(theta2); 
jb21 = sind(theta2); 
jb22 = cosd(alpha2)*cosd(theta2); 
jb23 = -sind(alpha2)*cosd(theta2);
jb24 = Link2*sind(theta2); 
jb31 = 0;
jb32 = sind(alpha2);
jb33 = cosd(alpha2);
jb34 = d2;
 
T2 = [jb11 jb12 jb13 jb14; jb21 jb22 jb23 jb24; jb31 jb32 jb33 jb34; 0 0 0 1];
 
jc11 = cosd(theta3); 
jc12 = -sind(theta3); 
jc13 = sind(alpha3)*sind(theta3);
jc14 = Link3*cosd(theta3); 
jc21 = sind(theta3); 
jc22 = cosd(alpha3)*cosd(theta3); 
jc23 = -sind(alpha3)*cosd(theta3);
jc24 = Link3*sind(theta3); 
jc31 = 0;
jc32 = sind(alpha3);
jc33 = cosd(alpha3);
jc34 = d3;
 
T3 = [jc11 jc12 jc13 jc14; jc21 jc22 jc23 jc24; jc31 jc32 jc33 jc34; 0 0 0 1];
 
T12 = T1*T2; 
T123 = T1*T2*T3;
 
% PLOTS
plot([0 T1(13)], [0 T1(14)], 'k','linewidth', 3);
hold on;
plot([T1(13) T12(13)], [T1(14) T12(14)], 'b','linewidth', 3);
hold on;
plot([T12(13) T123(13)], [T12(14) T123(14)], 'c','linewidth', 3);
hold on;
plot(0,0,'ko',T1(13),T1(14),'bo',T12(13),T12(14),'co','linewidth', 6);
hold on;
plot(T123(13),T123(14),'ro','linewidth',6);
 
L1 = num2str(Link1); 
L2 = num2str(Link2); 
L3 = num2str(Link3);
t1 = num2str(theta1); 
t2 = num2str(theta2); 
t3 = num2str(theta3);
x1 = num2str(T1(13)); 
y1 = num2str(T1(14));
x2 = num2str(T12(13)); 
y2 = num2str(T12(14));
x3 = num2str(T123(13)); 
y3 = num2str(T123(14));

grid on
A = ['Length of the first link: ' L1 '; Theta 1: ' t1 ' degrees'];
B = ['Length of the second link: ' L2 '; Theta 2: ' t2 ' degrees'];
C = ['Length of the third link: ' L3 '; Theta 3: ' t3 ' degrees'];
D = ['Joint 1: ( 0 ,  0 )']; 
E = ['Joint 2: (' x1 ', ' y1 ')'];
F = ['Joint 3: (' x2 ', ' y2 ')']; 
G = ['End effector: (' x3 ', ' y3 ')'];
legend(A,B,C,D,E,F,G); 
legend('location','NorthOutside');
title('Forward Kinematics of a 3-linked Robotic Arm using D-H Representation');

3. Conclusion

In this blog, we solve the location and coordinates of the end-effector using Denavit-Hartenberg convention in which the analysis lies respectively on each joint following the governing rules set by Denavit-Hartenberg representation. Also the Denavit-Hartenberg convention is representing both the translation and rotation in terms of these four variables θ_i,α_i,a_i and d_i. For a complex robotics system that involves translation and rotation. The Denavit-Hartenberg representation is preferable compared to calculating it manually using basic geometry and trigonometry analysis.

4. References

[1] Lynch, K. and F. Park. “Modern Robotics: Mechanics, Planning, and Control.” (2017).

[2] John J. Craig. 1989. Introduction to Robotics: Mechanics and Control (2nd. ed.). Addison-Wesley Longman Publishing Co., Inc., USA.

[3] Merat, Frank. (1987). Introduction to robotics: Mechanics and control. Robotics and Automation, IEEE Journal of. 3. 166 - 166. 10.1109/JRA.1987.1087086

(Note: All images in the text is drawn by the author except those with separate citation.)

Posted with STEMGeeks



0
0
0.000
0 comments