Thiết kế và mô phỏng điều khiển robot 3 bậc tự do rrr

Thiết kế và đặt hệ trục tọa độ cho mỗi khâu Do trình tự thiết kế và đặt hệ trục tọa độ các khâu tương tự nhau lên sau đây em xin trình bày rõ ràng thiết kế 1 khâu, còn các khâu khác em

NHIỆM VỤ ĐỒ ÁN MÔN HỌC

1 Đề Tài: Thiết kế và mô phỏng điều khiển robot 3 bậc tự do RRR

2 Cài công cụ cần thiết: Phần mềm Matlab, Solidworks

3 Các tài kiệu tham khảo

- Các tài liệu tham khảo phần mềm Matlab, công cụ Simulink, công cụ Simmechanics

- Các tài liệu về lí thuyết điều khiển

- Sách Robot công nghiệp, Động lực học hệ nhiều vật, …

4 Nội dung các phần thuyết minh và tính toán

- Vẽ và thiết kế Robot trên Solidworks

- Tính toán động học thuận và ngược

- Thiết kế điều khiển và mô phỏng trên Matlab

Họ và tên sinh viên: Phạm Duy Trọng MSSV: 20134134 – KT CĐT 02

Giảng viên hướng dẫn: TS Nguyễn Danh Trường

Bộ môn: Cơ học vật liệu và kết cấu

MỤC LỤC

Chương I: Thiết kế robot 3DOF – RRR trên phần mềm Solidwork

4

1 Thiết kế và đặt hệ trục tọa độ cho mỗi khâu 4

1.1 Thiết kế giá đỡ (khâu 0) 4

1.2 Thiết kế khâu 1 15

1.3 Thiết kế khâu 2 16

1.4 Thiết kế khâu 3 16

2 Lắp ráp các khâu 17

Chương II: Tính toán động học và động lực học cho robot 22

1 Thiết lập bảng thông số D-H cho robot 3DOF 22

2 Giải bài toán động học thuận 24

2.1 Ma trận D-H 24

2.2 Vận tốc góc các khâu 26

2.3 Gia tốc góc của khâu 30

3 Giải bài toán động học ngược 32

CHƯƠNG III: Thiết kế mạch mô phỏng điều khiển và GUI 39

1 Thiết kế mô phỏng điều khiển và GUI với version First Generation 39

1.1 Giới thiệu các khối của robot 41

1.2 Thêm các khối điều khiển 42

1.3 Kết quả chạy mô phỏng sau khi thêm các khối điều khiển 44

1.4 Lập trình giao diện GUI trong matlab 45

1.4.1 Thiết kế giao diện GUI 45

1.4.2 Lập trình trong giao diện file (.m) 48

2 Thiết kế mô phỏng điều khiển và GUI với version Second Generation 53

2.1 Giới thiệu các khối của robot 53

2.2 Thêm các khối điều khiển: PID, Step, S-PS, PS-S 54

2.3 Lập trình giao diện GUI trong matlab 57

2.3.1 Thiết kế giao diện GUI 57

2.3.2 Lập trình GUI trong file (.m) 63

Kết luận: 69

Chương I: Thiết kế robot 3DOF – RRR trên phần mềm Solidwork

1 Thiết kế và đặt hệ trục tọa độ cho mỗi khâu

Do trình tự thiết kế và đặt hệ trục tọa độ các khâu tương tự nhau lên sau đây em xin trình bày rõ ràng thiết kế 1 khâu, còn các khâu khác em xin phép được lấy hình

1.1 Thiết kế giá đỡ (khâu 0)

• Trên mặt phẳng Top vẽ sketch với biên dạng là đường tròn với D=300mm

• Sử dụng lệnh extrude

• Sử dụng lệnh chamfer:

• Trên mặt phẳng đáy vẽ sketch có biên dạng sau:

• Sử dụng lệnh extrude:

• Dùng lệnh chamfer

• Vẽ sketch trên mặt phẳng sau:

• Sau đó extrude:

• Tương tự vẽ sketch trên mặt phẳng sau và extrude:

Mặt phẳng

• Sử dụng lệnh mirror qua mặt phẳng front:

• Sử dụng lệnh cirpattern quanh trục y or tâm đường tròn

• Tạo plane1:

• Sau đó trên plane1 vẽ sketch với D = 200mm, và dưới mặt phẳng face1 vẽ đưởng tròn với D=220mm

• Dùng lệnh Loft để nối 2 đường tròn lại:

• Vẽ sketch trên mặt phẳng sau để tạo khớp quay:

• Đặt hệ tọa độ vào điểm point:

 Kết quả thu được chi tiết khâu 0:

1.2 Thiết kế khâu 1

1.3 Thiết kế khâu 2

1.4 Thiết kế khâu 3

• Ràng buộc bề mặt trùng nhau:

• Ràng buộc bề mặt song song:

 Chi tiết khâu 0 cố định:

• Tiếp tục lần lượt lấy các chi tiết khâu 1,2,3 vào môi trường để lắp ráp chúng lại

với nhau:

• Ràng buộc đồng tâm hay đồng trục và ràng buộc mặt của khâu 1 với khâu 0 lại

với nhau ta được:

• Tương tự ràng buộc khâu 2 với khâu 1

• Ràng buộc khâu 3 với khâu 2

Chương II: Tính toán động học và động lực học cho robot

1 Thiết lập bảng thông số D-H cho robot 3DOF

Bảng tham số DH của robot 3DOF-RRR

• Với các thông số biết trước như sau:

✓ Khâu 1: d1 = 900mm, khớp quay o0 với 1 =(-π,π)

✓ Khâu 2: a2 = 650mm, khớp quay o1 với 2 =( , )

3 3

✓ Khâu 3: a3 = 400mm, khớp quay o2 với 3 ( , )

3 3

• Cách chọn hệ trục tọa độ cho từng khâu theo D-H như sau:

✓ Trục zi được chọn dọc theo hướng của trục khớp động thứ i+1

✓ Trục xi được chọn dọc theo đường vuông góc chung của 2 trục zi 1 và

i

z , hướng từ trục zi 1 đến zi Nếu 2 trục zi 1 và zi cắt nhau thì hướng của trục xi được chọn tùy ý miễn là vuông góc với với trục zi Nếu 2 trục zi 1 và zi song song với nhau, thì chọn trục xi hướng theo pháp tuyến chung nào cũng được

✓ Trục yi được chọn sao cho hệ ( Oxyz )i là hệ quy chiếu thuận (khi quay trục Ox dương đến Oy dương và trục Oz còn lại dương thì gọi là hệ quy chiếu thuận)

✓ Gốc tọa độ oi được chọn tại giao điểm của xi và zi

✓ Đối với hệ tọa độ ( Oxyz )0 theo quy ước trên ta chọn được trục z0, còn 0

x được chọn tùy ý sao cho chúng vuông góc với nhau

2 Giải bài toán động học thuận

2.1 Ma trận D-H

- Ma trận D-H tổng quát:

( , ) (0,0, ) ( ,0,0) ( , )

- Ma trận T – Phương trình động học thuận của 3 khâu:

T3 là phương trình động học thuận của robot

 Tọa độ điểm cuối của khâu 3 là:

Trong đó: Ri được gọi là ma trận cosin chỉ hướng tương đối giữa khâu i so với i-1

- Suy ra vận tốc góc tương đối của khâu i so với i-1 là:

- Vận tốc góc tuyệt đối của các khâu (so với gốc đứng yên) là:

S C

2.3 Gia tốc góc của khâu

1 1 1 23 1 23 (0)

❖ Kết quả thu được bài toán thuận

✓ Tọa độ điểm cuối của khâu thao tác:

1

S C

✓ Gia tốc góc tuyệt đối của khâu thao tác:

3 Giải bài toán động học ngược

❖ Bài toán động học ngược là bài toán cho biết trước vị trí điểm thao tác C và hướng của khâu thao tác Yêu cầu: xác định các tọa độ khớp của khâu 1 và khâu

2

• Ma trận cấu hình của khâu thao tác:

24 (0)

34

C C C

1 24 1

24 1

arctan

❖ Kết quả thu được bài động học ngược

24 1

24 (0)

Từ bài toán động học thuận ta có ma trận D-H của khâu thao tác so với khâu cố định (khâu 0):

p S

p p

✓ Tìm góc 3

Ta lại có:

2 2

CHƯƠNG III: Thiết kế mạch mô phỏng điều khiển và GUI

1 Thiết kế mô phỏng điều khiển và GUI với version First Generation

- Run lệnh mech_import, đợi 1 lúc hiện lên của sổ:

Chọn đường dẫn đến file XML, sau đó chọn Apply và OK

Tiếp tục đợi 1 lúc trên matlab sẽ hiện ra chương trình thiết kế mạch điều khiển:

- Thu các khối không sử dụng đến thành 1 khối để nhìn cho gọn mạch điều khiển bằng cách chọn các khối không sử dụng, chọn phải chuột và chọn Creat Subsystem from Selection or Ctr + G

- Sau khi import và chỉnh sửa được từ file XML ta được các khối sau:

1.1 Giới thiệu các khối của robot

- Khâu đế - 1 cho ta biết chi tiết thông số của khâu 1 trong body

- Revolute là khối thể hiện khớp xoay 1 tại vị trí ban đầu, lưu ý: number of sensor – thể hiện số đầu vào và chỉnh thành 1

1.2 Thêm các khối điều khiển

- Khối constant: tạo các giá trị cố dịnh or phức tạp, mặc định giá trị là 1

- Khối slider gain: là khối điều khiển góc xoay cho các khớp

- Khối Joint Actuator: thể hiện các khớp nối

- Khối Mux: đùng để liên kết các tín hiệu vào thành đầu ra

1.3 Kết quả chạy mô phỏng sau khi thêm các khối điều khiển

Chú ý: chỉnh thông số thời gian chạy về inf (vô cùng)

1.4 Lập trình giao diện GUI trong matlab

1.4.1 Thiết kế giao diện GUI

Trong của sổ comment windowns gõ lệnh “guide” ta được giao diện sau:

Chọn Blank GUI và bắt đầu thiết kế:

- Theta1, Theta2, Theta3: là các góc trong bảng DH

- Ở đây e dùng 3 thanh slider1,2,3 để điều chỉnh các góc quay của khớp xoay 1,2,3 được chỉnh các thông số như sau: ví dụ slider1 xoay từ góc (-180,180)

- Các ô edit text dùng để lưu trữ và hiển thị các góc từ thanh slider truyền vào robot, ví dụ edit text1

Biến (thẻ) Slider1

Biến

edit1

Để trống

- Px, Py, Pz là tọa độ điểm cuối của khâu thao tác dùng để giải bài toán ngược

- Tương tự gắn với Px, Py, Pz bằng các thanh slider4,5,6 (điều chỉnh min, max của thanh slider tương ứng với Px, Py, Pz) tương ứng với edit text4,5,6

- Default: là nút pushbutton1 dùng để đưa robot về trạng thái ban đầu tức

là các góc theta1,2,3 = 0 deg

- Set: là nút tính toán bài toán ngược với biến là Pushbutton2

- Close: là nút đóng bảng điều khiển

❖ Chú ý: tất cả các tên khâu 1, khâu 2, khâu 3, px, py…ta dùng static text để đặt

và đây là thanh không phụ thuộc vào biến =>bỏ qua

1.4.2 Lập trình trong giao diện file (.m)

Sau khi ta thiết kế xong nhấn Run và lưu file (.gui ) vào thư mục Robot khi

đó ta sẽ có thêm một file m để lập trình và thêm Code để có thể điều khiển được robot

Biến

Pushbutton1

- Slider1_Callback: thêm code

function slider1_Callback(hObject, eventdata, handles) ModelName = 'robot3dof';

global var;%khai báo bien

slider1 gan vao bien t1

tri ve dang ki tu va dat vao thanh edti1

slider2 gan vao bien t2

tri ve dang ki tu va dat vao thanh edti2

slider3 gan vao bien t3

tri ve dang ki tu va dat vao thanh edti3

slider gain trong mach dk

slider gain trong mach dk

set_param([ModelName '/Slider

slider gain trong mach dk

b1=[cosd(t1) 0 sind(t1) 0 ;sind(t1) 0 -cosd(t1) 0 ;0 1 0

b2=[cosd(t2) -sind(t2) 0 650*cosd(t2) ;sind(t2) cosd(t2) 0

b3=[cosd(t3) -sind(t3) 0 400*cosd(t3);sind(t3) cosd(t3) 0

tri px thanh dang ki tu va dat vao thanh edti4

tri px thanh dang ki tu va dat vao thanh edti5

tri px thanh dang ki tu va dat vao thanh edti6

- Slider2, slider3 tương tự slider1

- Pushbutton1_Callback: Thêm code vào nút Default

function pushbutton1_Callback(hObject, eventdata, handles)

%dua robot ve vi tri ban dau t1=0,t2=0,t3=0

t1=[cosd(t1) 0 sind(t1) 0 ;sind(t1) 0 -cosd(t1) 0 ;0 1 0 135;0 0 0 1];

t2=[cosd(t2) -sind(t2) 0 100*cosd(t2) ;sind(t2) cosd(t2) 0 100*sind(t2) ;0 0 1 0 ;0 0 0 1];

t3=[cosd(t3) -sind(t3) 0 60*cosd(t3);sind(t3) cosd(t3) 0 60*sind(t3); 0 0 1 0 ; 0 0 0 1];

c=t1*t2*t3;

px=c(1,4);

py=c(2,4);

pz=c(3,4);

- Pushbutton2_Callback: Thêm code vào nút Set

function pushbutton2_Callback(hObject, eventdata, handles)

%bai toan dong hoc nguoc

ModelName = 'robot3dof';

global var; %khai bao bien

slider4 gan vào bien px

tri px thành ki tu va dat no vao thanh edit4

slider5 gan vào bien py

tri py thành ki tu va dat no vao thanh edit5

slider6 gan vào bien pz

tri pz thành ki tu va dat no vao thanh edit6

%%%%%%%tim cac goc theta

theta1 tim duoc chuyen thanh dang ki tu và dat gia tri do vao thanh edit1

silder gain trong mach dk

tren

- Pushbutton3_Callback: Thêm code vào nút close

function pushbutton3_Callback(hObject, eventdata, handles)

%nut dong bang dieu khien

close;

- Slider4_callback: thêm code

function slider4_Callback(hObject, eventdata, handles)

slider4 gan vao bien px

edit4

- Slider5,6,7 tương tự

❖ Giao diện bảng điều khiển được chạy như sau:

- Bài toán thuận:

- Bài toán nghịch:

2 Thiết kế mô phỏng điều khiển và GUI với version Second Generation

- Run lệnh smimport

- Trong môi trường matlab gõ lệnh smimport(‘file.xml’);

- Sau khi import được các khối robot

2.1 Giới thiệu các khối của robot

- Khối khaude_1_RIGID (các khâu còn lại tương tự)

✓ Solid: thông tin về đặc tính của khâu nhưu moomen, khối lượng, kích thước…

✓ RefecenFame: hệ tọa độ của khâu

✓ Transform: là khối thể hiện các biến đổi về tọa độ của khâu theo quy định tắc

DH

✓ Revolute: khớp xoay, chỉnh như sau

2.2 Thêm các khối điều khiển: PID, Step, S-PS, PS-S

- Khối PID (Tỷ lệ - Tích phân – Đạo hàm): là giải thuật điều khiển

được dùng nhiều nhất trong các ứng dụng điều khiển tự động với yêu cầu chính xác, nhanh và ổn định

=> PID là 1 cơ chế phản hồi vòng điều khiển (bộ điều khiển), nó sẽ tính toán giá trị sai số là hiệu số giữa giá trị đo thông số biến đổi và giá trị đặt mong muốn

❖ Chú ý: chọn tune để cho phần mềm tự động tính toán các thông số PID phù hợp

với robot

- Step: có chức năng xuất ra tín hiệu hàm bước và đây cũng là khối

truyền các tham số đầu vào là các góc theta cho robot

✓ Step time: khoảng thời gian đầu ra chuyển sang mức Final value kể từ lúc bắt

đầu mô phỏng Cài đặt giá trị này bằng 0

✓ Initial value: Giá trị ban đầu Cài đặt bằng 0

✓ Final value: Giá trị lúc sau Cài đặt theo giá trị ta muốn tác động tới hệ thống

✓ Sample time: thời gian lấy mẫu Cài đặt bằng 0

- PS-S: chuyển đổi các đầu vào tín hiệu vật lý thành một tín hiệu cho

khối Simulink

- S-PS: chuyển đổi tín hiệu từ khối Simulink thành một tín hiệu vật lý

- Gain: khối thu được từ tín hiệu đầu vào và chuyển đổi sang đầu ra

- Scope: khối thu thập dữ liệu theo các khối đã thực hiện hay hoạt

động

2.3 Lập trình giao diện GUI trong matlab

2.3.1 Thiết kế giao diện GUI

Trong của sổ comment windowns gõ lệnh “guide” ta được giao diện sau:

Chọn Blank GUI và bắt đầu thiết kế:

- Sử dụng công cụ thiết kế như:

✓ Push Button: là các nút bấm

✓ Edit Text: là nơi các kí tự được nhập vào từ người dùng, người dùng có thể dùng thay đổi được

✓ Slider: thanh trượt có 1 con trượt chạy trên đó

✓ Static Text: là các kí tự được hiện thị thông qua các Callback, hoặc thông thường để viết nhãn cho các biểu tượng, người dùng không thể thay đổi nội dung

- Ví dụ khâu 1:

✓ Nút tăng:

✓ Nút giảm:

✓ Edit Text:

2.3.2 Lập trình GUI trong file (.m)

Sau khi ta thiết kế xong nhấn Run và lưu file (.gui) vào thư mục Robot khi đó

ta sẽ có thêm một file m để lập trình và thêm Code để có thể điều khiển được robot

- Thêm code vào btnTangkhau1

function btnTangkhau1_Callback(hObject, eventdata,

handles)

% hObject handle to btnTangkhau1 (see GCBO)

% eventdata reserved - to be defined in a future version

gan bang bien handles.Goc1

handles.Goc2 = handles.Goc2; %gan bang bien handles.Goc2=0 deg

tri px thanh dang ki tu va dat vao thanh edti4

tri px thanh dang ki tu va dat vao thanh edti5

tri px thanh dang ki tu va dat vao thanh edti6

guidata(hObject,handles);

- btnTangkhau2, btnTangkhau3 chỉ khác với btnTangkhau1 ở chỗ:

✓ btnTangkhau2

handles.Goc1 = handles.Goc1;

gan bang bien handles.Goc2

handles.Goc3 = handles.Goc3;

set(handles.edit3,'string',num2str(handles.Goc3));

✓ btnTangkhau3

handles.Goc1 = handles.Goc1;

handles.Goc2 = handles.Goc2;

gan bang bien handles.Goc3

- Thêm code vào btnGiamkhau1 (tương tự với btnGiamkhau2, btnGiamkhau3): giống btnTangkhau1, btnTangkhau2, btnTangkhau3 chỉ khác ở chỗ thay dấu ‘+’ bằng dấu ‘-’, thay vì tăng 10deg bằng giảm 10deg

- btnStart: thêm code

function btnStart_Callback(hObject, eventdata, handles) handles = guidata(hObject);

global Tenrobot

Tenrobot='Robot3Dof';

khien

% Kiem tra load simulink model

load_system(handles.modelname);

end

set_param(handles.modelname,'BlockReduction','off');

set_param(handles.modelname,'StopTime','inf');

set_param(handles.modelname,'SimulationMode','normal');

% khi model start thi call funtion: localAddEventListener

ner');

% cho run simulink

set_param(handles.modelname,'SimulationCommand','start');

% an nut start

set(hObject,'Enable','off');

% hien nut stop

set(handles.btnStop,'Enable','on');

% save su thay doi bien toi GUI

guidata(hObject,handles);

- btnStop: thêm Code

function btnStop_Callback(hObject, eventdata, handles) set_param(handles.modelname,'SimulationCommand','stop'); set_param(handles.modelname,'StopTime','10');

set_param(handles.modelname,'StartFcn','');

set(hObject,'Enable','off');

set(handles.btnStart,'Enable','on');

set_param([handles.modelname '/Actuator1'],'after','0'); set_param([handles.modelname '/Actuator2'],'after','0'); set_param([handles.modelname '/Actuator3'],'after','0'); guidata(hObject,handles);

% Ham load su kien moi khi co bien thay doi

function localEventListener(block, eventdata)

drawnow;

- btnBTN: giải bài toán ngược