Date post: | 07-Feb-2017 |
Category: |
Documents |
Upload: | travis-heidrich |
View: | 64 times |
Download: | 0 times |
1
Model of Robotic Manipulator with Revolute and
Prismatic Joints
03/25/2016
Travis Heidrich
Report Contents:
Dimensions of the Robot 2
Denavit-Hartenberg Parameters 2-3
MATLAB Script 3-5
Frame Transformation Matrices 6
Reachable Workspace for Manipulator 7-8
Inverse Kinematics for Manipulator 8-9
Jacobian and Singularity 10
Angular and Linear Velocities, Forces, and Torques of Robot 11
2
Dimensions of Robot
L1 = 1 m; L2 = 0.5 sin(45Β°) = 0.3536 m; LE = 0.5 m
D-H Parameters
Figure 1: Reference Frames and Dimensions of the three link robot
3
Table 1: D-H Parameter Table for all frames of the robot
i Ξ±i - 1 ai - 1 di - 1 ΞΈi
1 0 0 L1 ΞΈ1 + 135Β°
2 L2 90Β° d2 0
3 0 0 0 ΞΈ3
E 0 -90Β° LE 0
MatLab Script
%Solutions clear all close all
%Robot Dimensions
L_1 = 1; %meters
L_2 = 0.5*sind(45); %meters L_E = 0.5; %meters
syms ai alphai di thetai theta1 d_2 theta3 T_x=[1 0 0 0;0 cos(alphai) -sin(alphai) 0;0 sin(alphai) cos(alphai) 0;0 0 0
1]; D_x=[1 0 0 ai;0 1 0 0;0 0 1 0;0 0 0 1]; T_z=[cos(thetai) -sin(thetai) 0 0;sin(thetai) cos(thetai) 0 0;0 0 1 0 ;0 0 0
1]; D_z=[1 0 0 0;0 1 0 0;0 0 1 di;0 0 0 1];
%Link the reference frames into one homogeneous transform matrix AtB=T_x*D_x*T_z*D_z;
%Transformation from frame 0 to frame 1 ai = 0; alphai = 0; di = L1; thetai = theta1 + 3*pi/4; T_01 = subs(AtB); iT_01 = inv(T_01);
%Transformation from frame 1 to frame 2 ai = L_2; alphai = pi/2; di = d_2; thetai = 0; T_12 = subs(AtB); %iT_12 = inv(T_12);
%Transformation from frame 2 to frame 3 ai = 0;
4
alphai = 0; di = 0; thetai = theta3; T_23 = subs(AtB);
%Transformation from frame 3 to frame E ai = 0; alphai = -pi/2; di = LE; thetai = 0;
T3E = subs(AtB); %iT3E = inv(T3E);
% Linking the Transformations T_02 = T_01*T_12; T_03 = T_01*T_12*T_23; T_0E = T_01*T_12*T_23*T_3E; T_1E = T_12*T_23*T_3E; T_2E = T_23*T_3E;
x = T_0E(1,4); y = T_0E(2,4); z = T_0E(3,4);
%Inverse Kinematics for robot syms px py pz theta3 = solve( pz == z, theta3); [d_2, theta1] = solve(px == x, py == y, d_2, theta1);
%Rotation Matrices frame to frame R_01 = T_01(1:3,1:3); R_12 = T_12(1:3,1:3); R_23 = T_23(1:3,1:3); R_3E = T_3E(1:3,1:3); R_02 = R_01*R_12; R_03 = R_01*R_12*R_23; R_0E = R_01*R_12*R_23*R_3E;
%Forces 10,5,and 1 f_EE = [10;5;1]; f_33 = R_3E*f_EE; f_22 = R_23*f_33; f_11 = R_12*f_22; f_00 = R_01*f_11;
%Vectors between D-H Origins P_01 = T_01(1:3,4); P_12 = T_12(1:3,4); P_23 = T_23(1:3,4); P_3E = T_3E(1:3,4); P_00 = [0;0;0]; P_02 = T_02(1:3,4); P_03 = T_03(1:3,4); P_0E = T_0E(1:3,4);
5
%Robot Torques n_EE = zeros(3,1); n_33 = R_01*n_EE + cross(P_3E,f_33); n_22 = R_01*n_33 + cross(P_23,f_22); n_11 = R_01*n_22 + cross(P_12,f_11); n_00 = R_01*n_11 + cross(P_01,f_00);
t_0 = transpose(n_00)*[0;0;1]; t_1 = transpose(n_11)*[0;0;1]; t_2 = transpose(f_22)*[0;0;1]; t_3 = transpose(n_33)*[0;0;1]; t_E = zeros(3,1);
%Velocities syms theta1Dot theta2Dot theta3Dot d_2Dot
%Unit Vectors u_i = [1;0;0]; u_j = [0;1;0]; u_k = [0;0;1];
%Angular rotations z_00 = u_k; z_01 = R_01*u_k; z_02 = R_02*u_k; %Prismatic z_03 = R_03*u_k;
J_angular = [z00, 0*z01, z02]; Omega_0E = theta1Dot*u_k + 0 + theta3Dot*R_02*u_k + 0;
%Linear J_linear = [cross(z_00,P_0E β P_00), z_02, cross(z_02,P_0E β P_02)];
%linear velocity equations W_00 = zeros(3,1); w_11 = inv(R_01)*w_00+[0;0;1]*theta1Dot; w_22 = inv(R_12)*w_11+[0;0;0]*theta2Dot; w_33 = inv(R_23)*w_22+[0;0;1]*theta3Dot; w_EE = zeros(3,1);
v_00 = zeros(3,1); v_11 = inv(R_01)*(v_00+cross(w_00,P_01)); v_22 = inv(R_12)*(v_11+cross(w_11,P_12))+d_2Dot*[0;0;1]; %Prismatic v_33 = inv(R_23)*(v_22+cross(w_22,P_23)); v_EE = inv(R_3E)*(v_33+cross(w_33,P_3E)); v_0E = R_0E*v_EE;
6
Frame Transformation Matrices
π01 =
[ βsin (
π
4+ π1) βcos (
π
4+ π1) 0 0
cos (π
4+ π1) β sin (
π
4+ π1) 0 0
0 0 1 10 0 0 1]
π12 =
[ 1 0 0
β2
40 0 β1 βπ2
0 1 0 00 0 0 1 ]
π23 = [
cos(π3) βsin(π3) 0 0
sin(π3) cos(π3) 0 00 0 1 00 0 0 1
]
π3πΈ =
[ 1 0 0 0
0 0 11
20 0 1 00 0 0 1]
π0πΈ =
[ β cos(π3) β sin (
π
4+ π1) βcos (
π
4+ π1) sin(π3) β sin (
π
4+ π1)
(sin(π3) β sin (π4
+ π1))
2+ π2 β cos (
π
4+ π1) β
(β2 β sin (π4
+ π1))
4
cos(π3) β sin (π
4+ π1) βπ ππ (
π
4+ π1) β cos (
π
4+ π1) β sin(π3)
(β2 β cos (π4
+ π1))
4β
(cos (π4
+ π1) β sin(π3))
2+ π2 β sin (
π
4+ π1)
sin(π3) 0 cos (π3)cos (π3)
2+ 1
0 0 0 1 ]
7
Reachable Workspace for Manipulator
Figure 2: X-Z Plane View of reachable workspace
Figure 3: X-Y Plane View of reachable workspace
8
Figure 4: Trimetric view of reachable workspace
Inverse Kinematics for Manipulator
π0πΈ = [
π11 π12 π13 ππ₯
π21 π22 π23 ππ¦
π31 π32 π33 ππ§
0 0 0 1
]
ππ§ =cos (π3)
2+1
ππ¦ = (β2 πππ (
π4 + π1))
4β
( πππ (π4 + π1) π ππ(π3))
2+ π2 π ππ (
π
4+ π1)
ππ₯ =( π ππ(π3) π ππ (
π4 + π1))
2β
(β2 π ππ (π4 + π1))
4+ π2 πππ (
π
4+ π1)
Solving for ΞΈ3
π3 = Β±cosβ1(2ππ§ β 2)
Substituting ΞΈ3 and solving for ΞΈ1 and d2
π1 = 2 tanβ1 (4ππ¦ Β± β2β(8ππ₯2 β 2β2 sin(π3) + cos(2π3) + 8ππ¦
2 β 2)) β3π
4
9
π2 = Β±
(β2β(8ππ₯2 + 8ππ¦
2 + cos(2π3) + 2β2 sin(π3) β 2))
4
Solutions for the point: (-0.275, 0.6125, 1.4375)
π3 = Β± 0.5054 πππ
π1
π2= {
β5.2426 0.1163 β1.7674 β0.8431 πππ0.6621 0.3099 β0.6621 β0.3099 π
Solution set to satisfy the limits is
π3 = 0.5054 πππ
π1 = β5.2426 πππ = 1.0406 πππ
π2 = 0.6621 π
Where d2 for both the offset L2 and the 0.1 m limit.
π2 β πΏ2 β 0.1 = 0.2085 π
Figure 5: MatLab output of robot configured with the inverse kinematic solution
10
Jacobian
[ cos (
π4
+ π1) β sin(π3)
2β
β2 β cos (π4
+ π1)
4β π2 β sin (
π
4+ π1) cos (
π
4+ π1)
cos(π3) β sin (π4
+ π1)
2
sin(π3) β sin (π4
+ π1)
2β
β2 β sin (π4
+ π1)
4+ π2 β cos (
π
4+ π1) sin(
π
4+ π1) β
cos(π3) β cos (π4
+ π1)
2
0 0β sin(π3)
2
0 0 βcos (π
4+ π1)
0 0 sin (π
4+ π1)
1 0 0 ]
π·ππ‘(π½ππππππ) = π2 β sin(π3)
2
π·ππ‘(π½ππππ’πππ) = 0
Singularities
When determinate of Jacobian = 0
The was no determinate for the angular Jacobian
There was no theta 1 that resulted in singularities
Given theta3 = 0 and pi
For d_2 < = 0.5(sin(45)) + 0.1
OR
d_2 > 0.5(sin(45)) + 0.5
11
Angular and Linear Velocities, Forces, and
Torques of Robot
Table 2: The angular velocity of the end effector
π0πΈ οΏ½ΜοΏ½3 β sin (
3π
4+ π1) βοΏ½ΜοΏ½3 β cos (
3π
4+ π1)
οΏ½ΜοΏ½1
Table 3: The linear velocity of the end effector
π£0πΈ π£π₯ =οΏ½ΜοΏ½1βsin (π1)
4β
οΏ½ΜοΏ½1βcos(π1)
4β
β2βοΏ½ΜοΏ½2βsin(π1)
2+
β2βοΏ½ΜοΏ½2βcos(π1)
2β
β2βπ1Μβsin(π1)βsin(π3)
4β
(β2βπ2βπ1Μβcos(π1))
2 β
(β2βπ2βπ1Μβsin(π1))
2+
β2βοΏ½ΜοΏ½3βcos(π1)βcos(π3)
4+
β2βοΏ½ΜοΏ½1βcos(π1)βsin(π3)
4
+β2βοΏ½ΜοΏ½3βcos(π3)βsin(π1)
4
π£π¦ =β2βοΏ½ΜοΏ½2βsin(π1)
2β
οΏ½ΜοΏ½1βsin(π1)
4β
οΏ½ΜοΏ½1βcos(π1)
4+
β2βοΏ½ΜοΏ½2βcos(π1)
2+
β2βοΏ½ΜοΏ½1βsin(π1)βsin(π3)
4+
β2βπ2βοΏ½ΜοΏ½1βcos(π1)
2β
β2βπ2βοΏ½ΜοΏ½1βsin(π1)
2β
β2βοΏ½ΜοΏ½3βcos(π1)βcos(π3)
4+
β2βοΏ½ΜοΏ½1βcos(π1)βsin(π3)
4+
β2βπ2βοΏ½ΜοΏ½3βcos(π3)βsin (π1)
4
π£π§ = βπ3Μsin (π3)
2
Table 4: The forces for each link with respect to each linkβs frame in N
End Effector πΉπ₯ = 10 πΉπ¦ = 5 πΉπ§ = 1
Third Link πΉπ₯ = 10 πΉπ¦ = 1 πΉπ§ = β5
Second Link πΉπ₯ = 8.266 πΉπ¦ = 5.716 πΉπ§ = β5
First Link πΉπ₯ = 8.266 πΉπ¦ = 5 πΉπ§ = 5.716
Base πΉπ₯ = -6.736 πΉπ¦ =-6.925 πΉπ§ = 5.716
Table 5: Each joint torque in Nm:
End Effector ππΈ = 0
Third Link π3 = β5
Second Link π2 = β5
First Link π1 =2.240
Base ππ© = 0