Scribd
Upload
20 views6 pages

Baocao TNROBOT

The document describes the forward and inverse kinematics problems of a robot. It presents the equations to solve for the joint angles given the end effector position and orientation. It also provides the equations to calculate the end effector pose given the joint angles.

Uploaded by

phat.dangbk2020
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
20 views6 pages

Baocao TNROBOT

The document describes the forward and inverse kinematics problems of a robot. It presents the equations to solve for the joint angles given the end effector position and orientation. It also provides the equations to calculate the end effector pose given the joint angles.

Uploaded by

phat.dangbk2020
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 6

ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH

TRƯỜNG ĐẠI HỌC BÁCH KHOA



BÁO CÁO
THÍ NGHIỆM KỸ THUẬT ROBOT

Sinh viên thực hiện:


Ngô Quốc Toàn – 1912224

– Thành phố Hồ Chí Minh –


BÀI TOÁN ĐỘNG HỌC ROBOTS

Bài toán động học thuận


function Plot_Frame(handles,theta1,theta2,theta3,theta4)
d1 = 0.077;
a2 = 0.128;
a3 = 0.024;
a4 = 0.124;
a5 = 0.126;
DH = [d1, theta1 , 0 , 90;
0 , theta2 + 90, a2, 0 ;
0 , -90 , a3, 0 ;
0 , theta3 , a4, 0 ;
0 , theta4 , a5, 0 ];
%% Homogeneous Transform
A0_0 = eye(4);
A0_1 = Matrix_Link(DH(1,1),DH(1,2),DH(1,3),DH(1,4));
A1_1p = Matrix_Link(DH(2,1),DH(2,2),DH(2,3),DH(2,4));
A1p_2 = Matrix_Link(DH(3,1),DH(3,2),DH(3,3),DH(3,4));
A2_3 = Matrix_Link(DH(4,1),DH(4,2),DH(4,3),DH(4,4));
A3_4 = Matrix_Link(DH(5,1),DH(5,2),DH(5,3),DH(5,4));
0
A0_1p = A0_1*A1_1p;
A0_2 = A0_1*A1_1p*A1p_2;
A0_3 = A0_1*A1_1p*A1p_2*A2_3;
A0_4 = A0_1*A1_1p*A1p_2*A2_3*A3_4;
%% Unit vector and Origin of Reference Frame
x0 = A0_0(1:3,1); y0 = A0_0(1:3,2); z0 = A0_0(1:3,3); % Unit vector X Y
Z of RF0
p0 = A0_0(1:3,4); % Postion of
Origin of RF0

x1 = A0_1(1:3,1); y1 = A0_1(1:3,2); z1 = A0_1(1:3,3); % Unit vector X Y


Z of RF1
p1 = A0_1(1:3,4); % Postion of
Origin of RF1

x1p = A0_1p(1:3,1); y1p = A0_1p(1:3,2); z1p = A0_1p(1:3,3); % Unit vector X


Y Z of RF1'
p1p = A0_1p(1:3,4); % Postion of
Origin of RF1'

x2 = A0_2(1:3,1); y2 = A0_2(1:3,2); z2 = A0_2(1:3,3); % Unit vector X Y


Z of RF2
p2 = A0_2(1:3,4); % Postion of
Origin of RF2

x3 = A0_3(1:3,1); y3 = A0_3(1:3,2); z3 = A0_3(1:3,3); % Unit vector X Y


Z of RF3
p3 = A0_3(1:3,4); % Postion of
Origin of RF3

xE = A0_4(1:3,1); yE = A0_4(1:3,2); zE = A0_4(1:3,3); % Unit vector X Y


Z of RF End Effector
pE = A0_4(1:3,4); % Postion of
Origin of RF End Effector

2
%% Print Reference Frame 0
if get(handles.checkbox1,'value')==1
Print_Reference_Frame(p0,x0,y0,z0,0);
end

%% Print Reference Frame 1


if get(handles.checkbox2, 'Value') == 1
Print_Reference_Frame(p1,x1,y1,z1,1);
end

%% Print Reference Frame 1'


if get(handles.checkbox3, 'Value') == 1
Print_Reference_Frame(p1p,x1p,y1p,z1p,1');
end

%% Print Reference Frame 2


if get(handles.checkbox4, 'Value') == 1
Print_Reference_Frame(p2,x2,y2,z2,2);
end

%% Print Reference Frame 3


if get(handles.checkbox5, 'Value') == 1
Print_Reference_Frame(p3,x3,y3,z3,3);
end

%% Print Reference Frame 4


if get(handles.checkbox5, 'Value') == 1
Print_Reference_Frame(pE,xE,yE,zE,4);
end
end

function Plot_Robot(handles,theta1,theta2,theta3,theta4)
global d1 a2 a3 a4 a5;
Parameter
DH = [d1, theta1 , 0 , 90;
0 , theta2 + 90, a2, 0 ;
0 , -90 , a3, 0 ;
0 , theta3 , a4, 0 ;
0 , theta4 , a5, 0 ];
A0_1 = Matrix_Link(DH(1,1),DH(1,2),DH(1,3),DH(1,4));
A1_1p = Matrix_Link(DH(2,1),DH(2,2),DH(2,3),DH(2,4));
A1p_2 = Matrix_Link(DH(3,1),DH(3,2),DH(3,3),DH(3,4));
A2_3 = Matrix_Link(DH(4,1),DH(4,2),DH(4,3),DH(4,4));
A3_4 = Matrix_Link(DH(5,1),DH(5,2),DH(5,3),DH(5,4));

A0_2 = A0_1*A1_1p*A1p_2;
A0_3 = A0_1*A1_1p*A1p_2*A2_3;
A0_4 = A0_1*A1_1p*A1p_2*A2_3*A3_4;

Min_X = str2double(get(handles.edit_Min_X,'String'));
Max_X = str2double(get(handles.edit_Max_X,'String'));

Min_Y = str2double(get(handles.edit_Min_X,'String'));
Max_Y = str2double(get(handles.edit_Max_X,'String'));

Min_Z = str2double(get(handles.edit_Min_Z,'String'));
Max_Z = str2double(get(handles.edit_Max_Z,'String'));

3
Min_View = str2double(get(handles.edit_view_min,'String'));
Max_View = str2double(get(handles.edit_view_max,'String'));

opacity = get(handles.slider_opacity,'value');
set(handles.edit_opacity,'string',num2str(100*opacity));

cla(handles.axes3,'reset');
rotate3d(handles.axes3,'on')
set(handles.axes3,'View', [Min_View, Max_View]);
axis([Min_X Max_X Min_Y Max_Y Min_Z Max_Z]);
hold(handles.axes3,'on');
grid(handles.axes3,'on');

Plot_Link_1(opacity,theta1);
Plot_Link_2(opacity,theta1,theta2);
Plot_Link_3(opacity,theta1,theta2,theta3);
Plot_Link_4(opacity,theta1,theta2,theta3,theta4);

RPY_1 = Roll_Pitch_Yaw(A0_1);
set(handles.text_X_1,'string',round(A0_1(1,4),3));
set(handles.text_Y_1,'string',round(A0_1(2,4),3));
set(handles.text_Z_1,'string',round(A0_1(3,4),3));
set(handles.text_Roll_1,'string',num2str(RPY_1(1),'%.3f'));
set(handles.text_Pitch_1,'string',num2str(RPY_1(2),'%.3f'));
set(handles.text_Yaw_1,'string',num2str(RPY_1(3),'%.3f'));

RPY_2 = Roll_Pitch_Yaw(A0_2);
set(handles.text_X_2,'string',round(A0_2(1,4),3));
set(handles.text_Y_2,'string',round(A0_2(2,4),3));
set(handles.text_Z_2,'string',round(A0_2(3,4),3));
set(handles.text_Roll_2,'string',num2str(RPY_2(1),'%.3f'));
set(handles.text_Pitch_2,'string',num2str(RPY_2(2),'%.3f'));
set(handles.text_Yaw_2,'string',num2str(RPY_2(3),'%.3f'));

RPY_3 = Roll_Pitch_Yaw(A0_3);
set(handles.text_X_3,'string',round(A0_3(1,4),3));
set(handles.text_Y_3,'string',round(A0_3(2,4),3));
set(handles.text_Z_3,'string',round(A0_3(3,4),3));
set(handles.text_Roll_3,'string',num2str(RPY_3(1),'%.3f'));
set(handles.text_Pitch_3,'string',num2str(RPY_3(2),'%.3f'));
set(handles.text_Yaw_3,'string',num2str(RPY_3(3),'%.3f'));

RPY_EE = Roll_Pitch_Yaw(A0_4);
set(handles.text_X_EE,'string',round(A0_4(1,4),3));
set(handles.text_Y_EE,'string',round(A0_4(2,4),3));
set(handles.text_Z_EE,'string',round(A0_4(3,4),3));
set(handles.text_Roll_EE,'string',num2str(RPY_EE(1),'%.3f'));
set(handles.text_Pitch_EE,'string',num2str(RPY_EE(2),'%.3f'));
set(handles.text_Yaw_EE,'string',num2str(RPY_EE(3),'%.3f'));

if get(handles.checkbox1,'value')==1
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end
if get(handles.checkbox1,'value')==0
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end

if get(handles.checkbox2,'value')==1

4
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end
if get(handles.checkbox2,'value')==0
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end

if get(handles.checkbox3,'value')==1
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end
if get(handles.checkbox3,'value')==0
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end

if get(handles.checkbox4,'value')==1
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end
if get(handles.checkbox4,'value')==0
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end

if get(handles.checkbox5,'value')==1
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end
if get(handles.checkbox5,'value')==0
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end

if get(handles.checkbox6,'value')==1
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end
if get(handles.checkbox6,'value')==0
Reference_Frame(handles,theta1,theta2,theta3,theta4)
end
end

Bài toán động học nghịch


function inverse(px, py, pz, pitch,t1, t2, t3, t4)
global t1
global t2
global t3
global t4
global d1
global a2
global a3
global a4
global a5
global px
global py
global pz
global pitch
warn = 0; %ve hinh, warn = 1: xuat hien canh bao
d1 = 7.7;
a2 = 13.0;
a3 = 12.4;
a4 = 12.6;
Phi = -pitch;
alpha = atan(12.8/2.4)*180/pi;

5
theta1 = atan2d(py, px);
t1 = theta1;
pr = sqrt(px^2+py^2);
Px = pr;
Py = pz - d1;

Pwx = Px - a4*cosd(Phi);
Pwy = Py - a4*sind(Phi);
c3 = (Pwx^2 + Pwy^2 - a2^ 2 - a3^2)/(2*a2*a3);
s3 = [-sqrt(1-c3^2), sqrt(1-c3^2)];
%theta3_ = atan2d(s3, [c3,c3]);
theta3_ = atan2d(s3, c3);
theta3 = theta3_ + alpha;

c2 = ((a2 + a3*cosd(theta3_))*Pwx +
(a3*sind(theta3_))*Pwy)/(Pwx^2+Pwy^2);
s2 = ((a2 + a3*cosd(theta3_))*Pwy -
(a3*sind(theta3_))*Pwx)/(Pwx^2+Pwy^2);
theta2_ = atan2d(s2, c2);
theta2 = theta2_ - alpha;
theta4 = Phi - theta2_ - theta3_;

t2 = theta2;
t3 = theta3;
t4 = theta4;

end

You might also like

pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy