BÁO cáo môn học kỹ THUẬT ROBOT đề tài THIẾT kế và điều KHIỂN CÁNH TAY ROBOT 4 bậc tự DO

1.6 Kết nối matlab simulink: Mỗi khớp có 3 đầu vào là vị trí góc, vận tốc góc, gia tốc góc.. Do mô phỏng động học của robot nên cung cấp đầu vào vị trí góc, vận tốc góc và gia tốc góc là

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP.HỒ CHÍ MINH

KHOA ĐIỆN – ĐIỆN TỬ Môn học: KỸ THUẬT ROBOT

BÁO CÁO MÔN HỌC KỸ THUẬT ROBOT

ĐỀ TÀI: THIẾT KẾ VÀ ĐIỀU KHIỂN CÁNH TAY

ROBOT 4 BẬC TỰ DO

GVHD: TS NGUYỄN VĂN THÁI Sinh viên thực hiện: Nhóm 10

Tp Hồ Chí Minh, tháng 06 năm 2022

Thiết kế mô hình trên Solidworks:

Mô hình robot gồm 5 phần

Tổng quan mô hình cánh tay robot 4 bậc tự do

Hệ trục 0, 1, 2, 3, 4.

1.1 Phần đế:

Hệ tọa độ gốc:

● Trục Z hướng lên, trùng với chiều quay của robot

● Trục X hướng sang phải

● Trục Y hướng vào trong

Hệ tọa độ 3:

● Trục Z hướng ra ngoài màn hình

● Trục X hướng sang phải

● Trục Y hướng lên trên

1.6 Kết nối matlab simulink:

Mỗi khớp có 3 đầu vào là vị trí góc, vận tốc góc, gia tốc góc

Do mô phỏng động học của robot nên cung cấp đầu vào vị trí góc, vận tốc góc và gia tốc góc

là hằng số

● a: là chiều dài (là khoảng cách giữa 2 trục động cơ Zi với Zi+1).

● : là độ xoắn (được xác định bằng độ lệch giữa 2 trục động cơ Zi với Zi+1)

● d: là độ lệch (là khoảng cách giữa 2 mặt phẳng chứa xi với xi+1 cùng vuông góc vớitrục Z)

● : là góc khớp

Thông số:

● Ma trận tổng quát chuyển đổi từ hệ i sang hệ i+1:

● Ma trận chuyển đổi từ hệ 0 sang hệ 1:

● Ma trận chuyển đổi từ hệ 1 sang hệ 2:

● Ma trận chuyển đổi từ hệ 2 sang hệ 3:

● Ma trận chuyển đổi từ hệ 3 sang hệ 4:

● Ma trận chuyển đổi từ hệ 0 sang hệ 4:

Làm tương tự ta được:

2.2.4 Tính theta4

Đặt: θ2 +θ3 +θ4=0

θ4=θ234−θ2−θ3

3 Thiết kế giao diện:

3.1 Giao diện giới thiệu:

Code giao diện giới thiệu:

function varargout = START(varargin)

3.2 Giao diện điều khiển:

Chức năng giao diện:

● Nút START: chạy file simulink mô hình robot

● Nút STOP: dừng file simulink mô hình robot

● Nút RESET: đặt lại giá trị của các góc theta về 0

● Nút SOLVE: tính toán giá trị góc theta 1,2,3,4 từ vị trí Px, Py, Pz cho trước

● Nhập góc mô phỏng bằng 2 cách: kéo thanh trượt hoặc nhập trực tiếp

Code giao diện điều khiển:

function varargout = FINAL_PROJECT(varargin)

% Tinh FK

P = Forward_kinematic(theta1,theta2,theta3,theta4);

Px = P(1); Py=P(2); Pz=P(3);

%hien thi cac gia tri Px_inv,Py_inv,Pz_inv len cac o hien thi gia tri Px_inv,Py_inv,Pz_inv trongguide

function varargout = FINAL_PROJECT_OutputFcn(hObject, eventdata,

handles) varargout{1} = handles.output;

% silder theta1

function slider_theta1_Callback(hObject, eventdata,

handles) theta1 = get(handles.slider_theta1,'value');

%hien thi cac gia tri Px_inv,Py_inv,Pz_inv len cac o hien thi gia tri Px_inv,Py_inv,Pz_inv trongguide

function slider_theta2_Callback(hObject, eventdata,

handles) theta1 = get(handles.slider_theta1,'value');

if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))

set(hObject,'BackgroundColor',[.9 9 9]);

end

%% slider theta3

function slider_theta3_Callback(hObject, eventdata,

handles) theta1 = get(handles.slider_theta1,'value');

function slider_theta4_Callback(hObject, eventdata,

handles) theta1 = get(handles.slider_theta1,'value');

set(handles.theta1,'string',num2str(theta1)); theta2 =

get(handles.slider_theta2,'value');

function theta1_inv_Callback(hObject, eventdata, handles)

function theta1_inv_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');

end

function theta2_inv_Callback(hObject, eventdata, handles)

function theta2_inv_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');

end

function theta3_inv_Callback(hObject, eventdata, handles)

function theta3_inv_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

function theta4_inv_Callback(hObject, eventdata, handles)

function theta4_inv_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

%//////////////////////////////////////////////////////////////////////

function Px_inv_Callback(hObject, eventdata, handles)

function Px_inv_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

function Py_inv_Callback(hObject, eventdata, handles)

function Py_inv_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

function Pz_inv_Callback(hObject, eventdata, handles)

function Pz_inv_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

theta234_new = str2num(theta234_new);

% Tinh IK

theta = Inverse_kinematic(Px_new,Py_new,Pz_new,theta234_new); set(handles.theta1_inv,'string',num2str(theta(1)));

set_param(handles.stop,'Enable','on');

%% kiem tra load file

function modelLoaded = modelIsLoaded(modelName)

try

modelLoaded =

~isempty(find_system('Type','block_diagram','Name',modelName)); catch ME

function theta1_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

function theta3_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

%//////////////////////////////////////////////////////////////////////

function theta4_Callback(hObject, eventdata, handles)

theta4_new=get(handles.theta4,'String');

function theta4_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

%//////////////////////////////////////////////////////////////////////

function theta234_Callback(hObject, eventdata, handles)

function theta234_CreateFcn(hObject, eventdata, handles)

if ispc && isequal(get(hObject,'BackgroundColor'),

get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white');end

3.2.1 Hàm con tính động học thuận:

function P = Forward_kinematic(theta1,theta2,theta3,theta4)

%% thong so mo hinh

function theta = Inverse_kinematic(Px,Py,Pz,theta234)

% khai bao thong so L1 = 150;

s2 = (ny*(L3*c3+L2)-L3*s3*nx)/((L3*c3+L2)^2+L3^2*s3^2);theta2 = atan2d(s2,c2);