Professional Documents
Culture Documents
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
• Insert/move
• Đọc, tạo, xuất bản vẽ. ft
• Reference: tạo các tham chiếu: point, face, line
Để luyện tập thiết kế robot trên SolidWorks, sinh viên có thể tham khảo khoá học
a
SolidWorks: Design for Mechatronics của Justin Flett tại LinkedLearning:
https://www.linkedin.com/learning/solidworks-design-for-mechatronics/
Hoặc xem các video hướng dẫn tại đây. Lưu ý khoá học trên sử dụng SolidWorks 2018 nên
dr
các file .sldprt của động cơ bước Nema 17 có thể không tương thích với phiên bản
SolidWorks mà sinh viên sử dụng.
4. Các lệnh cơ bản trong Matlab
• Kiểu dữ liệu trong Matlab
• Tính toán ma trận
• Hàm solve()
• Hàm subs()
• Khối chuyển đổi tín hiệu: Simulink-PS Converter, PS-Simulink Converter
• Khối Slider gain
• Khối nguồn Step
• Chuyển đổi mô hình từ SolidWorks sang Matlab
• Mô phỏng cánh tay trên Matlab
• Giao diện điều khiển GUI
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
Nhắc lại ma trận biến đổi thuần nhất mô tả hướng và vị trí của hệ tọa độ gắn trên khâu thứ 𝑖 so
a ft
với hệ tọa độ gắn trên khâu thứ 𝑖 − 1:
𝐴𝑖 = 𝑅𝑜𝑡(𝑧𝑖−1 , 𝜃𝑖 ) × 𝑇𝑟𝑎𝑛𝑠(0,0, 𝑑𝑖 ) × 𝑇𝑟𝑎𝑛𝑠(𝑎𝑖 , 0,0) × 𝑅𝑜𝑡(𝑥𝑖 , 𝛼𝑖 )
dr
Hình 2. Minh hoạ việc chuyển đổi HTĐ gắn vào khâu thứ 𝑖 so với HTĐ gắn vào khâu thứ 𝑖 − 1
Thứ tự các phép biến đổi:
▪ Quay quanh 𝑧𝑖−1 1 góc 𝜃𝑖
▪ Tịnh tiến dọc theo 𝑧𝑖−1 1 đoạn 𝑑𝑖
▪ Tịnh tiến dọc theo 𝑥𝑖−1 1 đoạn 𝑎𝑖
▪ Quay quanh 𝑥𝑖−1 (≡ 𝑥𝑖 ) 1 góc 𝛼𝑖
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
(a) Minh hoạ trong mặt phẳng 2D (b) Minh hoạ 3D (có vấn đề trên hình vẽ)
Hình 3. Thiết lập các hệ toạ độ cho robot
3
𝜃1 90
𝜃2
𝜃3
0
0
0
𝑎2
𝑎3
𝒅𝒊
𝑑1
0
dr
Các ma trận biến đổi thuần nhất 𝐴𝑖 :
𝐴1 = 𝑅𝑜𝑡(𝑧0 , 𝜃1 ) × 𝑇𝑟𝑎𝑛𝑠(0,0, 𝑑1 ) × 𝑅𝑜𝑡(𝑋1 , 𝛼1 )
𝐴2 = 𝑅𝑜𝑡(𝑧1 , 𝜃2 ) × 𝑇𝑟𝑎𝑛𝑠(𝑎2 , 0,0)
𝐴3 = 𝑅𝑜𝑡(𝑧2 , 𝜃3 ) × 𝑇𝑟𝑎𝑛𝑠(𝑎3 , 0,0)
Ma trận biến đổi thuần nhất của hệ toạ độ gắn trên khâu chấp hành cuối của tay máy so với hệ
tọa độ gốc:
𝑇 = 𝐴1 ∗ 𝐴2 ∗ 𝐴3
Kết quả tính trong Matlab:
Nếu khâu chấp hành cuối của tay máy được gắn một hệ tọa độ như Hình 4:
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
Hình 4. Hệ tọa độ đặt vào khâu chấp hành cuối của tay máy
Với:
𝑛𝑥 𝑜𝑥 𝑎𝑥
𝑅 = [𝑛𝑦 𝑜𝑦 𝑎𝑦 ] là ma trận cosin chỉ hướng của hệ {𝑂3} so với hệ {O}
𝑛𝑧 𝑜𝑧 𝑎𝑧
𝑝𝑥
𝑝
𝒑 = [ 𝑦 ] là vector tọa độ tâm tay gắp 𝑂3 trong hệ tọa độ gốc.
𝑝𝑧
Khi đó ma trận 𝑇 có dạng:
a ft 𝑛𝑥
𝑛
𝑇=[ 𝑦
𝑛𝑧
0
𝑜𝑥
𝑜𝑦
𝑜𝑧
0
𝑎𝑥 𝑝𝑥
𝑎𝑦 𝑝𝑦
𝑎𝑧 𝑝𝑧
0 1
]
dr
5.2. Bài toán động học thuận, ngược
• Điều kiện ràng buộc các biến khớp: hạn chế bài toán đa nghiệm.
−180 < 𝜃1 ≤ 180
0 ≤ 𝜃2 ≤ 90
−180 ≤ 𝜃3 ≤ 0
• Bài toán động học thuận: cho giá trị 3 biến khớp 𝜃1 , 𝜃2 , 𝜃3 tìm tọa độ vector p.
cos 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
𝒑 = [ sin 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 ) ]
𝑑1 + 𝑎3 sin(𝜃2 + 𝜃3 ) + 𝑎2 sin 𝜃2
• Bài toán động học ngược: cho tọa độ vector p tìm giá trị 3 biến khớp 𝜃1 , 𝜃2 , 𝜃3 .
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
cos 𝜃1 =
sin 𝜃1 =
𝑝𝑥
√𝑝𝑥2 + 𝑝𝑦2
𝑝𝑦
√𝑝𝑥2 + 𝑝𝑦2
dr
𝑝𝑦
tan 𝜃1 =
𝑝𝑥
Chọn nghiệm 𝜃1 dựa vào hình sau:
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
𝑝𝑥 =
ft
cos 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
𝒑 = [𝑝𝑦 ] = [ sin 𝜃1 (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 ) ]
𝑝𝑧 𝑑1 + 𝑎3 sin(𝜃2 + 𝜃3 ) + 𝑎2 sin 𝜃2
𝑝𝑥
√𝑝𝑥2+ 𝑝𝑦2
(𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
dr
𝑝𝑦
𝑝𝑦 = (𝑎3 cos(𝜃2 + 𝜃3 ) + 𝑎2 cos 𝜃2 )
√𝑝𝑥2 + 𝑝𝑦2
{ 𝑝𝑧 = 𝑑1 + 𝑎3 sin(𝜃2 + 𝜃3 ) + 𝑎2 sin 𝜃2
𝑝𝑟 = √𝑝𝑥2 + 𝑝𝑦2
a {
ft
02 là giao điểm giữa đường tròn 𝑐1 tâm 𝑂1 bán kính 𝑎2 và đường tròn 𝑐2 tâm 𝑂3 bán kính 𝑎3 :
𝑟 2 + (𝑧 − 𝑑1 )2 = 𝑎22
(𝑟 − 𝑝𝑟 )2 + (𝑧 − 𝑝𝑧 )2 = 𝑎32
Hệ này cho ra hai nghiệm 𝑂2 (𝑟2 ; 𝑧2 ) và 𝑂′2 (𝑟′2 ; 𝑧′2 )
Ta có:
dr
𝑟2
cos 𝜃2 =
𝑎2
𝑝𝑟 − 𝑟2
cos 𝛽 =
𝑎3
𝜃3 = −(𝜃2 + 𝛽)
A2=Rotz(t2)*Trans(a2,0,0);
A3=Rotz(t3)*Trans(a3,0,0);
%ma tran bien doi thuan nhat
T=A1*A2*A3;
%don gian hoa cac bieu thuc
T=simplify(T)
%trich phan tu cot 4
p=T(:,4)
function y = Rotx(t)
%phep quay quanh truc x
y=[1 0 0 0;0 cosd(t) -sind(t) 0; 0 sind(t) cosd(t) 0;0 0 0 1];
end
function y = Rotz(t)
%phep quay quanh truc z
y=[cos(t) -sin(t) 0 0; sin(t) cos(t) 0 0;0 0 1 0;0 0 0 1];
end
function a = Trans(x,y,z)
a
%phep tinh tien theo truc x, y, z
ft
a=[1 0 0 x;0 1 0 y; 0 0 1 z; 0 0 0 1]; %trong bai toan robot y = 0
end
dr
5.3. Bản vẽ các khâu robot
Độ dài các khâu được cho như sau:
Bảng 2. Bảng thông số các khâu của robot
Khâu 𝜽𝒊 𝜶𝒊 𝒂𝒊 𝒅𝒊
1 𝜃1 90 0 𝑑1 = 180𝑚𝑚
2 𝜃2 0 𝑎2 = 250𝑚𝑚 0
3 𝜃3 0 𝑎3 = 200𝑚𝑚 0
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
a ft
dr
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
Trong đó:
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
• Các tiêu đề theta1, theta2, theta3, px, py, pz là các Slider gain từ 1-6. Khi dịch chuyển
trên thanh siler gain, giá trị của nó sẽ thay đổi. Các slider gain có giá trị là “num”.
Tác vụ 1: Di chuyển slider1/2/3 → xuất giá trị cho edit1/2/3
Tác vụ 2: Di chuyển slider4/5/6 → xuất giá trị cho edit4/5/6
• 6 edit text từ 1-6 ngoài chức năng hiển thị, còn có chức năng lấy giá trị từ bàn phím.
Kiểu dữ liệu là “string”.
Tác vụ 3: Nhập giá trị edit1/2/3 → xuất giá trị cho slider1/2/3
Tác vụ 4: Nhập giá trị edit4/5/6 → xuất giá trị cho slider4/5/6
• Các tiêu đề solve1, solve2, reset, close là các nút nhấn pushbutton từ 1-4. Khi kích hoạt
nút nhấn, các tác vụ sẽ chạy trong hàm của nó.
Tác vụ 5: Nhấn pushbutton1 → lấy giá tri từ silder1/2/3 → giải bài toán động học thuận → xuất
giá trị cho edit4/5/6 → xuất giá trị cho slider4/5/6 → xuất giá trị cho gain/gain1/gain2 → chạy
mô phỏng trên SimMechanics.
Tác vụ 6: Nhấn pushbutton2 → lấy giá trị từ silder4/5/6 → giải bài toán động học nghịch →
•
•
•
ft
xuất giá trị cho edit1/2/3 → xuất giá trị cho slider1/2/3 → gán giá trị cho gain/gain1/gain2 →
chạy mô phỏng trên SimMechanics.
Tác vụ 7: Nhấn pushbutton3→ reset slider1/2/3 edit1/2/3 về 0 → gọi lại hàm pushbutton1
a
Tác vụ 8: Nhấn pushbutton4→ gọi hàm “close”
Bài toán động học thuận: tác vụ 1 hoặc 3 → tác vụ 5.
Bài toán động học nghịc: tác vụ 2 hoặc 4 → tác vụ 6.
Vị trí ban đầu → tác vụ 7.
dr
• Đóng cửa sổ → tác vụ 8.
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
if nargout
gui_mainfcn(gui_State, varargin{:});
end
a ft
[varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
dr
function gui_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject handle to figure
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
% varargin command line arguments to gui (see VARARGIN)
handles.output = hObject;
%cac dieu kien dau cho slider gain(neu can)
%set(handles.slider1, 'Min', -180);
%set(handles.slider1, 'Max', 180);
%set(handles.slider1, 'SliderStep', [10/360 , 10/360 ]);
%set(handles.slider2, 'Min', -90);
%set(handles.slider2, 'Max', 180);
%set(handles.slider2, 'SliderStep', [10/270 , 10/270 ]);
%set(handles.slider3, 'Min', -180);
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
a
% hObject handle to slider1 (see GCBO)
ft
function slider1_Callback(hObject, eventdata, handles)
%
a ft
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
end
set(hObject,'BackgroundColor',[.9 .9 .9]);
a ft
function slider4_Callback(hObject, eventdata, handles)
Tác vụ 2.1
dr
% hObject handle to slider3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a=get(hObject,'value');%lay gia tri slider4
set(handles.edit4,'string',num2str(a));%xuat gia tri cho edit4
% Hints: get(hObject,'Value') returns position of slider
% get(hObject,'Min') and get(hObject,'Max') to determine range of slider
ft
function slider5_Callback(hObject, eventdata, handles)
a ft
function edit4_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit3 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
dr
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
ft
% eventdata reserved - to be defined in a future version of MATLAB
% handles structure with handles and user data (see GUIDATA)
a
a=get(hObject,'string');%lay gia tri edit6
set(handles.slider6,'value',str2num(a));%xuat gia tri cho slider6
dr
function edit6_CreateFcn(hObject, eventdata, handles)
% hObject handle to edit5 (see GCBO)
% eventdata reserved - to be defined in a future version of MATLAB
% handles empty - handles not created until after all CreateFcns called
% Hint: edit controls usually have a white background on Windows.
% See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
set(hObject,'BackgroundColor','white');
end
pz=get(handles.slider6,'value');
%dieu kien xac dinh goc theta 1
if px==0&&py<0
t1=-90;
elseif px==0&&py>=0
t1=90;
elseif py==0&&px>=0
t1=0;
elseif py==0&&px<0
t1=180;
elseif (px>0&&py>0)||(px>0&&py<0)
t1=radtodeg(atan(py/px));
elseif px<0&&py>0
t1=180+radtodeg(atan(py/px));
else t1=-180+radtodeg(atan(py/px));
end
%xac dinh goc theta 2, theta 3
a ft
syms t2 t3;%tao bien hinh thuc symbolic variables
pr=sqrt(px^2+py^2);
ps=sqrt((pz-180)^2+pr^2);
dr
f1=200*cos(t2+t3 )+ 250*cos(t2)-pr;
f2=200*sin(t2+t3)+250*sin(t2)+180-pz;
[t2,t3]=solve(f1,f2,t2,t3);%dung solve giai bai toan dong hoc nghich
t=[t2/pi*180 t3/pi*180];%chuyen qua radian, ma tran t chua 2 cap nghiem
t=double(t);%dua ve dang so thap phan double
%dieu kien chon nghiem theta 2,theta3
if ps==450||(t(1,1)>t(2,1))
t2=t(1,1);
t3=t(1,2);
else t2=t(2,1);t3=t(2,2);
end
%xuat gia tri cho edit1/2/3
set(handles.edit1,'string',num2str(t1));
set(handles.edit2,'string',num2str(t2));
set(handles.edit3,'string',num2str(t3));
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
Tác vụ 7
dr
set(handles.edit3,'string',0);
%goi lai cac ham edit1/2/3
edit1_Callback(@edit1_Callback, eventdata, handles);
edit2_Callback(@edit2_Callback, eventdata, handles);
edit3_Callback(@edit3_Callback, eventdata, handles);
%goi lai ham pushbutton1(solve1)
pushbutton1_Callback(@pushbutton1_Callback, eventdata, handles);
%update lai simulink
%set_param('robot_RRR', 'SimulationCommand', 'stop');
set_param('robot_RRR', 'SimulationCommand', 'update');
%set_param('robot_RRR', 'SimulationCommand', 'start');
BÀI GIẢNG MÔ HÌNH HÓA ROBOT – Cập nhật 10/2021
GV: TS. Lê Hoài Nam – Trợ giảng: Lê Xuân Hoàng, Ngô Văn Hoàng Phúc, Thái Bá Hòa
a ft
dr