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
Bạn đang xem bản rút gọn của tài liệu. Xem và tải ngay bản đầy đủ của tài liệu tại đây (4.68 MB, 39 trang )
<span class="text_page_counter">Trang 1</span><div class="page_container" data-page="1">
<b>TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINHKHOA ĐIỆN - ĐIỆN TỬ</b>
<b>BỘ MƠN ĐIỀU KHIỂN TỰ ĐỘNG</b>
</div><span class="text_page_counter">Trang 2</span><div class="page_container" data-page="2"><b>LỜI CẢM ƠN</b>
Sau thời gian hơn 2 tháng nhận đề tài cũng và hồ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 hố. Đượ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 q trình học các bộ môn liên quan đến ngành điều khiển và tự động hố, 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 ngồi chun 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 chun mơn.
Đó là tất cả những điều chung nhất nhóm em rút ra sau khi hồ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.
</div><span class="text_page_counter">Trang 3</span><div class="page_container" data-page="3"><b>TÓM TẮT ĐỀ TÀI </b>
Đề 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.
</div><span class="text_page_counter">Trang 4</span><div class="page_container" data-page="4"><b>CHƯƠNG 1. THIẾT KẾ VÀ THI CƠNG MƠ HÌNH...7</b>
<b>1.1. Thiết kế và xây dựng mơ hình...7</b>
<b>1.1.1. Thiết kế link 0 (Base) robot...7</b>
<b>1.1.2. Kích thước link 1 robot...8</b>
<b>1.1.3. Kích thước link 2 robot...8</b>
<b>1.1.4. Kích thước link 3 robot...9</b>
<b>1.2. Thiết bị sử dụng trong mơ hình...9</b>
<b>CHƯƠNG 2. TÍNH TỐN VÀ KIỂM NGHIỆM ĐỘNG HỌC...12</b>
<b>2.1. Tính tốn động học thuận...12</b>
<b>2.1.1. Đặt trục toạ độ cho hệ cánh tay robot...12</b>
<b>2.1.2. Lập bảng D – H (Denavit – Hartenberg)...13</b>
<b>2.1.3. Ma trận chuyển đổi...13</b>
<b>2.2. Tính tốn động học nghịch cho cánh tay robot 3 bậc tự do...14</b>
<b>2.3. Kiểm chứng động học trên Matlab Simulink...16</b>
<b>CHƯƠNG 3. Chương trình và giao diện điều khiển...17</b>
<b>3.1. Sơ đồ khối điều khiển cánh tay robot 3 bậc tự do...17</b>
<b>3.2. Thiết kế giao diện...17</b>
<b>CHƯƠNG 4. KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI...20</b>
<b>4.1. Kết luận...20</b>
<b>4.2. Hướng phát triển đề tài...20</b>
<b>TÀI LIỆU THAM KHẢO...21</b>
<b>PHỤ LỤC...22</b>
</div><span class="text_page_counter">Trang 5</span><div class="page_container" data-page="5"><b>DANH MỤC BẢNG</b>
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
</div><span class="text_page_counter">Trang 6</span><div class="page_container" data-page="6"><b>DANH MỤC HÌNH Ả</b>
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
</div><span class="text_page_counter">Trang 7</span><div class="page_container" data-page="7"><b>CHƯƠNG 1. THIẾT KẾ VÀ THI CƠNG MƠ HÌNH</b>
<b>1.1. Thiết kế và xây dựng mơ hình </b>
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
<b>1.1.1. Thiết kế link 0 (Base) robot</b>
Hình 1. 2 Link base của Robot
</div><span class="text_page_counter">Trang 8</span><div class="page_container" data-page="8"><b>1.1.2. Kích thước link 1 robot </b>
Hình 1. 3 Link 1 của Robot
<b>1.1.3. Kích thước link 2 robot </b>
Hình 1. 4 Link 2 của Robot
</div><span class="text_page_counter">Trang 9</span><div class="page_container" data-page="9"><b>1.1.4. Kích thước link 3 robot</b>
Hình 1. 5 Link 3 của Robot
<b>1.2. Thiết bị sử dụng trong mơ hình </b>
Bảng 1. 1 Thiết bị sử dụng trong mơ hình
</div><span class="text_page_counter">Trang 11</span><div class="page_container" data-page="11"><b>CHƯƠNG 2. TÍNH TỐN VÀ KIỂM NGHIỆM ĐỘNG HỌC</b>
<b>2.1. Tính tốn động học thuận </b>
22Equation Section (Next)Các bước tính tốn động học thuận của cánh tay robot 3 bậc tự do được trình bày như sau:
<b>2.1.1. Đặt trục toạ độ cho hệ cánh tay robot</b>
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
</div><span class="text_page_counter">Trang 12</span><div class="page_container" data-page="12">i
ad
<b>Trong đó:</b>
a
<sub> là chiều dài của mỗi khâu được xác định bằng đường vuông chung</sub> 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
<sub> là độ lệch của khớp được xác định bằng đường vuông góc chung hay</sub> khoảng cách giữa hai mặt phẳng chứa trục tọa độx
<small>i</small><sub> và </sub>x
<sub>i</sub><sub>1</sub><sub>. là góc khớp được</sub> xác định bằng góc lệch giữa hai khâu liền kề.<b>2.1.3. Ma trận chuyển đổi </b>
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]:
<small>1</small>
</div><span class="text_page_counter">Trang 13</span><div class="page_container" data-page="13">+ Ma trận chuyển đổi từ hệ 2 sang hệ 3:
<b>2.2. Tính tốn động học nghịch cho cánh tay robot 3 bậc tự do.</b>
Mục đích của bài tố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 tố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:
</div><span class="text_page_counter">Trang 16</span><div class="page_container" data-page="16"><b>2.3. Kiểm chứng động học trên Matlab Simulink</b>
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 tốn ra vị trí đầu cuối của Robot rồi ta đưa vào động học nghịch để tính tố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 tố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 tốn động học thuận
Hình 2.4: Phương trình tốn động học nghịch cho Robot
</div><span class="text_page_counter">Trang 17</span><div class="page_container" data-page="17"><b>CHƯƠNG 3. CHƯƠNG TRÌNH VÀ GIAO DIỆN ĐIỀU KHIỂN </b>
<b>3.1. Sơ đồ khối điều khiển cánh tay robot 3 bậc tự do. </b>Đ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 tố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
<b>3.2. Thiết kế giao diện </b>
Hình 3. 2 Giao diện chính
</div><span class="text_page_counter">Trang 18</span><div class="page_container" data-page="18">Giao diện chính gồm 1 nút nhấn “Close”, có chức năng thố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 tố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
</div><span class="text_page_counter">Trang 19</span><div class="page_container" data-page="19">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 tố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.
</div><span class="text_page_counter">Trang 20</span><div class="page_container" data-page="20"><b>CHƯƠNG 4. KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN ĐỀ TÀI</b>
<b>4.1. Kết luận </b>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 tố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ị. Hồn thành việc đồng bộ 3 động cơ.
<b>4.2. Hướng phát triển đề tài </b>
</div><span class="text_page_counter">Trang 21</span><div class="page_container" data-page="21"><b>TÀI LIỆU THAM KHẢO</b>
[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.
</div><span class="text_page_counter">Trang 22</span><div class="page_container" data-page="22">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;
</div><span class="text_page_counter">Trang 23</span><div class="page_container" data-page="23">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
</div><span class="text_page_counter">Trang 24</span><div class="page_container" data-page="24">// 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;
</div><span class="text_page_counter">Trang 25</span><div class="page_container" data-page="25">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;
</div><span class="text_page_counter">Trang 26</span><div class="page_container" data-page="26">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)
</div><span class="text_page_counter">Trang 27</span><div class="page_container" data-page="27">digitalWrite(stepperPin1, HIGH)
</div><span class="text_page_counter">Trang 29</span><div class="page_container" data-page="29">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;
</div><span class="text_page_counter">Trang 30</span><div class="page_container" data-page="30">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);
</div><span class="text_page_counter">Trang 32</span><div class="page_container" data-page="32">// Thiết lập kết nối với cổng COM
private void onButton_Click(object sender, EventArgs e)
</div><span class="text_page_counter">Trang 33</span><div class="page_container" data-page="33">double Px_Nghich = 0, Py_Nghich = 0, Pz_Nghich = 0;
double the1_DHN = 0, the2_DHN = 0, the3_DHN = 0;
</div><span class="text_page_counter">Trang 35</span><div class="page_container" data-page="35">//=== 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) /
</div><span class="text_page_counter">Trang 36</span><div class="page_container" data-page="36">// 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); }
</div><span class="text_page_counter">Trang 37</span><div class="page_container" data-page="37">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)
</div><span class="text_page_counter">Trang 38</span><div class="page_container" data-page="38">-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)
</div><span class="text_page_counter">Trang 39</span><div class="page_container" data-page="39">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" +
</div>
