thực tập kỹ thuật robot thiết kế và điều khiển cánh tay robot 3 bậc tự do

Đồng thời tạo cho nhóm em hình dung chân thực nhất về công việc sẽ gắn bó với mình sau này.Về mặt kiến thức, nhóm em đã trau dồi một lượng kiến thức đáng kể về ngành điều khiển và tự độn

TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP HỒ CHÍ MINHKHOA ĐIỆN - ĐIỆN TỬ

BỘ MÔN ĐIỀU KHIỂN TỰ ĐỘNG

LỜI CẢM ƠN

Sau thời gian hơn 2 tháng nhận đề tài cũng và hoàn thành đồ án, bản thân nhóm em đã đúc kết ra những điều sau:

Bản thân mình thực sự yêu thích ngành học điều khiển và tự động hoá Được nhận chuyên đề giúp cho nhóm em có cảm giác trải nghiệm một phần nào công việc, nhiệm vụ của một người kỹ sư điều khiển tự động là như thế nào Đồng thời tạo cho nhóm em hình dung chân thực nhất về công việc sẽ gắn bó với mình sau này.

Về mặt kiến thức, nhóm em đã trau dồi một lượng kiến thức đáng kể về ngành điều khiển và tự động hoá, khắc sâu những kiến thức nhóm em đã trau dồi trong suốt quá trình học các bộ môn liên quan đến ngành điều khiển và tự động hoá, tìm hiểu thêm được những điều hữu ích của bộ môn từ nhiều nguồn: thực tế, sách, internet, hướng dẫn từ thầy và giúp đỡ của các bạn, và có thể đem kiến thức đó sau này ứng dụng vào công việc và cuộc sống.

Về mặt kỹ năng, bản thân nhóm em đã nắm được nhiều kỹ năng bổ ích như phân tích số liệu, trình bày vấn đề dưới dạng văn bản, kỹ năng sử dụng các phần mềm hỗ trợ như MATLAB, Visual Studio và cả kỹ năng tìm kiếm thông tin và xử lý thông tin từ nhiều nguồn.

Kiến thức lĩnh vực chuyên môn hay ngoài chuyên môn là vô cùng rộng lớn và quý giá nếu chúng ta biết sử dụng đúng cách Bản thân phải cố gắng hơn nữa để nắm vững hơn vốn kiến thức được trang bị trong nhà trường và cần phải trang bị cho mình những hành trang tri thức bằng việc đọc sách báo, các tài liệu kỹ thuật có liên quan đến chuyên ngành Đồng thời phải học hỏi thêm từ thầy cô, anh - chị những người đi trước cũng như bạn bè thuộc lĩnh vực chuyên môn.

Đó là tất cả những điều chung nhất nhóm em rút ra sau khi hoàn thành thực tập Robot Nhóm em xin chân thành cảm ơn Thầy Nguyễn Văn Thái - người đã tận tình hướng dẫn, giúp đỡ, tạo mọi điều kiện cho chúng em hoàn thành tốt thực tập Robot.

TÓM TẮT ĐỀ TÀI

Đề tài nhóm chúng em tập trung thiết kế phần cứng, tính động học thuận và động học nghịch, thiết kế giao diện thu thập và hiển thị dữ liệu.

CHƯƠNG 1 THIẾT KẾ VÀ THI CÔNG MÔ HÌNH 7

1.1 Thiết kế và xây dựng mô hình 7

1.1.1 Thiết kế link 0 (Base) robot 7

1.1.2 Kích thước link 1 robot 8

1.1.3 Kích thước link 2 robot 8

1.1.4 Kích thước link 3 robot 9

1.2 Thiết bị sử dụng trong mô hình 9

CHƯƠNG 2 TÍNH TOÁN VÀ KIỂM NGHIỆM ĐỘNG HỌC 12

2.1 Tính toán động học thuận 12

2.1.1 Đặt trục toạ độ cho hệ cánh tay robot 12

2.1.2 Lập bảng D – H (Denavit – Hartenberg) 13

2.1.3 Ma trận chuyển đổi 13

2.2 Tính toán động học nghịch cho cánh tay robot 3 bậc tự do 14

2.3 Kiểm chứng động học trên Matlab Simulink 16

CHƯƠNG 3 Chương trình và giao diện điều khiển 17

3.1 Sơ đồ khối điều khiển cánh tay robot 3 bậc tự do 17

3.2 Thiết kế giao diện 17

CHƯƠNG 4 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI 20

4.1 Kết luận 20

4.2 Hướng phát triển đề tài 20

TÀI LIỆU THAM KHẢO 21

PHỤ LỤC 22

DANH MỤC BẢNG

Bảng 1 1 Thiết bị sử dụng trong mô hình 9 Bảng 1 2 Bảng thông số vật lí 12 Bảng 1 3 Bảng D - H 13

DANH MỤC HÌNH Ả

Hình 1 1 Mô hình Robot 3 bậc tự do 7

Hình 1 2 Link base của Robot 7

Hình 1 3 Link 1 của Robot 8

Hình 1 4 Link 2 của Robot 8

Hình 1 5 Link 3 của Robot 9Y Hình 3 1 Sơ đồ khối điều khiển cánh tay Robot 17

Hình 3 2 Giao diện chính 17

Hình 3 3 Giao diện hiển thị thông số để vận hành Robot 18

CHƯƠNG 1 THIẾT KẾ VÀ THI CÔNG MÔ HÌNH

1.1 Thiết kế và xây dựng mô hình

11Equation Section 1Mô hình cánh tay robot 3 bậc tự do được thiết kế trên phần mềm SolidWorks như hình sau:

Hình 1 1 Mô hình Robot 3 bậc tự do

1.1.1 Thiết kế link 0 (Base) robot

Hình 1 2 Link base của Robot

1.1.2 Kích thước link 1 robot

Hình 1 3 Link 1 của Robot

1.1.3 Kích thước link 2 robot

Hình 1 4 Link 2 của Robot

1.1.4 Kích thước link 3 robot

Hình 1 5 Link 3 của Robot

1.2 Thiết bị sử dụng trong mô hình

Bảng 1 1 Thiết bị sử dụng trong mô hình

CHƯƠNG 2 TÍNH TOÁN VÀ KIỂM NGHIỆM ĐỘNG HỌC

2.1 Tính toán động học thuận

22Equation Section (Next)Các bước tính toán động học thuận của cánh tay robot 3 bậc tự do được trình bày như sau:

2.1.1 Đặt trục toạ độ cho hệ cánh tay robot

Hình 2.1: Đặt hệ trục tạo độ cho Robot Bảng 1 2 Bảng thông số vật lí

L1 = 115.16 mm Khoảng cách từ đế đến hệ trục tọa độ 1 theo trục Z L2 = 248.96 mm Khoảng cách từ đế đến hệ trục tọa độ 1 theo trục X

i ad

Trong đó:a là chiều dài của mỗi khâu được xác định bằng đường vuông chung giữa hai trục Z là độ xoắn của mỗi khâu được xác định bằng góc lệch giữa hai trục Z d là độ lệch của khớp được xác định bằng đường vuông góc chung hay khoảng cách giữa hai mặt phẳng chứa trục tọa độ xi và xi1 là góc khớp được xác định bằng góc lệch giữa hai khâu liền kề.

2.1.3 Ma trận chuyển đổi

Ta tiến hành thay lần lượt các giá trị trong bảng D-H vào ma trận chuyển đổi sau: Ta có ma trận chuyển đổi tổng quát từ hệ thứ i sang hệ thứ i+1 [1]:

1

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

2.2 Tính toán động học nghịch cho cánh tay robot 3 bậc tự do.

Mục đích của bài toán động học nghịch là tìm các biến khớp của tay máy khi biết vị trí khâu tác động cuối của tay máy Có 2 phương pháp để giải bài toán động học nghịch là phương pháp hình học và phương pháp đại số Ở đây nhóm đã áp dụng phương pháp đại số để giải Các bước thực hiện được trình bày dưới đây:

Chọn hướng của end effector là nên ta có:

Ta nhân các ma trận chuyển đổi như sau:

2.3 Kiểm chứng động học trên Matlab Simulink

Hình 2.2: Kiểm chứng động học trên Matlab Simulink

Ta đưa các giá trị đặt là các góc ban đầu vào hệ thống qua Forward Kinematic sẽ tính toán ra vị trí đầu cuối của Robot rồi ta đưa vào động học nghịch để tính toán đưa ra các góc xoay theo từng khớp của Robot Từ hình trên ta nói rằng phương trình động học của ta tính toán đã đúng và ta có tới tận 2 bộ nghiệm cho hệ phương trình với hướng của vị trí đầu cuối Robot là -90 độ.

Hình 2.3: Phương trình toán động học thuận

Hình 2.4: Phương trình toán động học nghịch cho Robot

CHƯƠNG 3 CHƯƠNG TRÌNH VÀ GIAO DIỆN ĐIỀU KHIỂN 3.1 Sơ đồ khối điều khiển cánh tay robot 3 bậc tự do

Điều khiển cánh tay robot 3 bậc tự do dựa trên sơ đồ Trong đó khối giao diện có chức năng tuỳ chỉnh các thông số robot và các tuỳ chọn điều khiển robot từ đó gửi bộ góc qua Arduino thông giao tiếp serial (UART) Khối vi điều khiển có chức năng nhận bộ góc, xử lí, tính toán sau đó tạo ra các xung để cung cấp điều khiển từng step motor Từng động cơ step sẽ gắn vào mạch Arduino CNC Shield V3 nhận tín hiệu trả về từ Arduino và thực hiện chuyển động trong robot.

Hình 3 1 Sơ đồ khối điều khiển cánh tay Robot

3.2 Thiết kế giao diện

Hình 3 2 Giao diện chính

Giao diện chính gồm 1 nút nhấn “Close”, có chức năng thoát khỏi chương trình Bên cạnh đó, giao diện điều khiển thể hiện đầy đủ tên Giáo viên hướng dẫn, tên các sinh viên thực hiện đề tài và Logo của nhà trường.

Hình 3 3 Giao diện hiển thị thông số để vận hành Robot

Nhằm hướng tới sự đơn giản và dễ dàng điều khiển đối với người vận hành cũng như người dùng, nhóm thiết kế giao diện điều khiển rất đơn giản nhưng vẫn đầy đủ các thông tin để điều khiển và vận hành một Robot.

Giao diện điều khiển được chia thành 4 phần chính:

Phần thứ 1: “COM PORT” tại Groupbox này sẽ bao gồm những nội dung khi kết nối Arduino với máy tính, gồm 2 nút nhấn “CONNECT” và “STOP”, cho phép người sử dụng kết nối cũng như ngắt kết nối giữa Arduino và máy tính Nút nhấn “NHÀ” sẽ setup cho robot về vị trí Home mà nhóm đã thiết lập.

Phần thứ 2: Thông tin về nhóm, gồm tên nhóm và các thành viên tham gia thực hiện đồ án.

Phần thứ 3: “FORWARD KINEMATIC” đây là groupbox để tính toán và giải động học thuận của robot, bao gồm 3 thanh TrackBar, cho phép người sử dụng kéo và nhập các góc của robot Nút nhấn “CACULATE FK” có chức năng giải động học thuận và đưa ra các giá trị vị trí của robot tại các Textbox Px, Py, Pz

bên cạnh Nút nhấn “COPY” cho phép người dùng sao chép giá trị vị trí của robot và dán vào các Textbox ở phần động học nghịch.

Phần thứ 4: “INVERSE KINEMATIC” đây là Groupbox để tính toán và giải động học thuận của robot, các Textbox Px, Py, Pz có thể được nhập từ động học thuận hoặc ta có thể dễ dàng sao chép giá trị bằng cách nhấn nút “COPY” Sau khi có các giá trị vị trí của robot, ta sẽ nhấn nút “CACULATE IK”, nút này sẽ giải động học nghịch của robot, sau đó sẽ đưa ra giá trị các góc theta từ 1 tới 3 tương ứng với vị trí mà giá trị chúng ta đã nhập.

CHƯƠNG 4 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI 4.1 Kết luận

Nhóm chúng em đã làm được những điều sau đây:

Hoàn thành thiết kế SolidWork và thi công mô hình Robot 3 bậc tự do Tính toán được động học thuận và động học nghịch

Thiết kế thành công giao diện hiển thị Hoàn thành việc đồng bộ 3 động cơ

4.2 Hướng phát triển đề tài

TÀI LIỆU THAM KHẢO

[1] J J Craig, Introduction to Robotics Mechanics and Control Pearson Education International.

[2] K S FU, ROBOTICS: Control, Sensing, Vision, and Intelligence The United States of America, 1987.

[3] K Z A War War Naing, Aung Thike, "Position Control of 3-DOF Articulated Robot Arm using PID Controller," International Journal of Science and Engineering Applications, vol 7, no 09,254-259, 2018.

[4] B S L Sciavicco and L V G Oriolo, Robotics Modelling, Planning and Control Springer, 2009.

[5] P T N T THỊNH, GIÁO TRÌNH KỸ THUẬT ROBOT NHÀ XUẤT BẢN ĐẠI HỌC QUỐC GIA THÀNH PHỐ HỒ CHÍ MINH, 2014.

[6] H Z Hossein Sadegh Lafmejani, "Modeling, Simulation and Position Control of 3DOF Articulated Manipulator," Indonesian Journal of Electrical Engineering and Informatics (IJEEI), vol 2, 2014.

String step1, step2, step3;

int16_t goc_step1 = , goc_step2 = , goc_step3 = 0 0 0;

int16_t goc_step1_t = , 0goc_step2_t = 0goc_step3_t = ;0

int16_t goc_step1_t1 = , goc_step2_t1 = , goc_step3_t1 = 0 0 0;

bool flag_run = false;

bool flag_reset = false;

bool flag_relay = false;

pinMode(Y_DIR, OUTPUT) pinMode(Y_STP, OUTPUT) pinMode(Z_DIR, OUTPUT) pinMode(Z_STP, OUTPUT)

pinMode(EN, OUTPUT); digitalWrite(EN, LOW);

pinMode(Relay, OUTPUT) pinMode(Switch_t1, INPUT_PULLUP) pinMode(Switch_t2, INPUT_PULLUP) pinMode(Switch_t3, INPUT_PULLUP) digitalWrite(Relay, LOW)

// set Home cho robot

// Trong đó xxx là s xung c n phát đ i v i các stepố ầ ố ớ

if(inByte != 'r' && inByte != 'h' && inByte != 'n')

//=== Chay chuong trinh hut vat ===//

if(flag_relay == true)digitalWrite(Relay, 1) else digitalWrite(Relay, 0);

//=== B t đ u ch y tr ng thái Set home ===// ắ ầ ạ ạ

if(flag_reset == true)

flag_reset = false;

indexofs = data_rec indexOf ('s') indexofa = data_rec indexOf ('a') indexofb = data_rec indexOf ('b') indexofc = data_rec indexOf ('c')

// Serial.println(data_rec);

step1 = data_rec.substring(indexofs + , indexofa1 ) step2 = data_rec.substring(indexofa + , indexofb1 ) step3 = data_rec.substring(indexofb + , indexofc1 )

//Serial.println((String)step1 + " " + (String)step2 + " " + (String)step3);

data_rec = "";

goc_step1_t = step1 toInt (); goc_step2_t = step2 toInt (); goc_step3_t = step3 toInt (); goc_step1 = goc_step1_t - goc_step1; goc_step2 = goc_step2_t - goc_step2; goc_step3 = goc_step3_t - goc_step3;

if(goc_step2 >= huong_step2 = 0) 1; else huong_step2 = 0;

if(goc_step3 >= huong_step3 = 0) 1; else huong_step3 = 0;

// DDieu khien step quay

func_step(huong_step1, X_DIR, X_STP, huong_step2, Y_DIR, Y_STP, huong_step3, Z_DIR, Z_STP,

goc_step1, goc_step2, goc_step3, 1000)

// L uư giá tr ch y tr c đó là bi n g c step và bi n nh n giá tr

voidfunc_step(boolean dir1, byte dirPin1, byte stepperPin1, boolean dir2, byte dirPin2, byte stepperPin2, boolean dir3, byte dirPin3, byte stepperPin3,

intsteps1, intsteps2, intsteps3 , intdelays) {

// Ch n h ng cho t ng step motor ọ ướ ừ

digitalWrite(dirPin1, dir1) digitalWrite(dirPin2, dir2) digitalWrite(dirPin3, dir3)

digitalWrite(stepperPin1, HIGH)

if(digitalRead(Switch_t2) == LOW)

func_step_xung(Inverse, Z_DIR, Z_STP, 3000, 1000);

goc_step1 = ; goc_step2 = ; goc_step2 = 0 0 0; goc_step1_t = 0; goc_step2_t = ; 0goc_step3_t = 0;

GraphPane myGraph_SS = new GraphPane(); // Dat cac bien ban dau

// Chieu dai cac Link Robot

Int16 L1 = 30, L2 = 30, L3 = 20, L4 = 15, d1 = 5;

// Khoi tạo bien trang thai chon che do Run DHT hay DHN

delegate void serialCalback(string val);

// Dat cac bien ban dau

string[] ports = SerialPort.GetPortNames(); boxCOMPORT.Items.AddRange(ports);

Set_The1.Text = Convert.ToString(0); Set_The2.Text = Convert.ToString(0); Set_The3.Text = Convert.ToString(0);

// Thiết lập kết nối với cổng COM

private void onButton_Click(object sender, EventArgs e)

double Px_Nghich = 0, Py_Nghich = 0, Pz_Nghich = 0;

double the1_DHN = 0, the2_DHN = 0, the3_DHN = 0;

//=== Tính Theta 2 của Robot ===//

// the1_DHN đang là chế độ gốc -> Math.Cos or Math.Sin dùng chế độ Rad

A1 = Px * Math.Cos(the1_DHN * (Math.PI) / 180) + Py * Math.Sin(the1_DHN * (Math.PI) /

// gui tin hieu step di

serialPort1.Write("s" + Convert.ToString(step1) + "a" + Convert.ToString(step2) + "b" +

// Giá trị các gốc theta theo thanh trượt

private void slider_the1_Scroll(object sender, EventArgs e) {

val_the1 = this.slider_the1.Value; Set_The1.Text = Convert.ToString(val_the1); }

private void slider_the2_Scroll(object sender, EventArgs e)

val_the1_rad = (val_the1 * (Math.PI)) / 180; val_the2_rad = (val_the2 * (Math.PI)) / 180; val_the3_rad = (val_the3 * (Math.PI)) / 180;

// chuyen gia tri ve rad

Px = L1 * Math.Cos(val_the1_rad) + d1 * Math.Sin(val_the1_rad) + L3 * Math.Cos(val_the1_rad) * Math.Cos(val_the2_rad) +

L4 * (Math.Cos(val_the1_rad) * Math.Cos(val_the2_rad) * Math.Cos(val_the3_rad)

-Math.Cos(val_the1_rad) * Math.Sin(val_the2_rad) * Math.Sin(val_the3_rad)); Px = Math.Round(Px, 2);

Py = L1 * Math.Sin(val_the1_rad) - d1 * Math.Cos(val_the1_rad) + L3 * Math.Sin(val_the1_rad) * Math.Cos(val_the2_rad) +

L4 * (Math.Sin(val_the1_rad) * Math.Cos(val_the2_rad) * Math.Cos(val_the3_rad) -Math.Sin(val_the1_rad) * Math.Sin(val_the2_rad) * Math.Sin(val_the3_rad));

Int16 step1, step2, step3; Int16 step1_t, step2_t, step3_t; bool tt_gui = false;

private void BT_Send_DHT_Click(object sender, EventArgs e)

if ((step3_t <= (step3 - 10)) || (step3_t >= (step3 + 10))) // gui tin hieu step di

serialPort1.Write("s" + Convert.ToString(step1) + "a" + Convert.ToString(step2) + "b" +