Structure and Programming Design for Dual arm Robot

Structure and Programming Design for Dual arm Robot

Nattawut Hansupo

การออกแบบงานโครงสร้าง สำหรับติดตั้งหุ่นยนต์ 2 แขน (Dual Arm Robot)

จาก conceptual ในการทำระบบ ได้มีการออกแบบโครงสร้างโดยให้ตัวหุ่นยนต์มีการยืดอยู่บนโครงสร้าง และด้านหน้าเป็น Table

ในการวาง Component ต่างๆ อาทิ ขวดน้ำ แก้วน้ำ รวมถึง Camera ที่ใช้ในการ Detect Object

โดยโครงสร้างแบ่งออกเป็น 2 ส่วน คือ

1. โครงส้รางสำหรับติดจั้งหุ่นยนต์ 2 แขน

การออกแบบได้มีการคำนึงถึงน้ำหนักของหุ่นยนต์ ทั้ง2แขนเพื่อให้โครงส้รางที่ประกอบขึ้นมาโดยใช้วัสดุจาก อลูมิเนียมโปรไฟล์

ให้สามารถรองรับน้ำหนักได้  รวมถึงระยะในการทำงาน Work space ได้มีการออกแบบโดยดูจากระยะ reach ของหุ่นยนต์

และความยาวของ Gripper เพื่อให้สามารถไปหยิบชิ้นงานได้

Ref: https://catalog.fa.com.my/UR5

ผลการ Simulation

Conclusion

จากการ Simulation โครงสร้างมีค่า Yield Strength  และ Factor of safety ที่สามารถทำให้โครงสร้างนี้รองรับน้ำหนักหุ่นยนต์ได้

2.Table สำหรับวางชิ้นงาน

การออกแบบ จะใช้ Table ที่มี 2 ชั้น โดยใช้ล่างจะใช้สำหรับการวางขวดน้ำ และวางแก้ว และชั้นที่ 2 สำหรับการหยิบแก้วน้ำและขวดน้ำมาวาง รวมถึงใช้เป็นจุดติดตั้ง Camera สำหรับ Detect object

ภาพหน้างานจริงที่มีการติดตั้งหุ่นยนต์เข้ากับโครงสร้างและ Table

Budget

Software Design for Dual arm Robot

โครงสร้างการทำ Software Design

Software สำหรับการออกแบบการควบคุมหุ่นยนต์  (ณัฐวุฒิ)

จะมีการทำโปรแกรมอยู่ 6 ส่วน คือ

1. Inverse Kinematics Function : รับค่าจากกล้องที่มีการหา Position และ transformation เข้ามาเพื่อทำการนำไปเข้าสู้โปรแกรม Inverse Kinematics
2. Forward Kinematics Function : ในการทำจะใช้สมการจากที่มีการ prove มาจากพิธา เพื่อนำมาสร้างเป็น function และนำไปใช้ในการหา joint ของหุ่นยนต์หลังจากที่มีการขยับโดย User และนำไปเข้า Inverse Kinematics เพื่อขยับหุ่นยนต์
3. Manual Function : เป็นส่วนที่เอาไว้ใช้ในการรับค่าจาก User เพื่อสั่งให้หุ่นยนต์ขยับ โดยในการทำโปรแกรมจะเป็นการใช้คำสั่ง Keyboard
4. Teach Robot Function : เป็นการเขียนคำสั่งให้หุ่นยนต์ขยับไปตามจุดต่างที่เราผมการ Set เอาไว้ตาม Path รวมถึงการรับส่งค่าจาก Function ที่มีการเขียนไว้ข้างต้น
5. Trajectory Function : จากโปรแกรม Inverse Kinematics เราจะได้ค่าแกนทั้งหมดที่จะต้องขยับ และนำไปเข้าสู่โปรแกรม Trajectory ในการหาจุดที่จะผ่านและ accelerate and speed แต่ในส่วนนี้ ยังไม่สามารถทำได้ จึงใช้วิธีการกำหนดความเร็วและความเร่งเป็นค่าคงที่ ในการทดสอบโปรแกรม
6. Path planning Function :จากนั้นจะทำการขยับตาม Path planning ซึ่งในการทำ Program ในกาทดสอบ ใช้การกำหนดเป็น Linear Equations และซอยจุด Via point ออกเป็น 20 จุดเพื่อนำค่าไปตรวจสอบหา singularity ก่อนขยับไปยังจุดต่อไป

Flow ของการทำงานของหุ่นยนต์ 2 แขน

การควบคุมแขนกลโรบอทผ่าน RTDE (Real-Time Data Exchange)

ในสภาพแวดล้อม Python โดยใช้โมดูลthreading, socket,numpy, rtde_control, และ rtde_receive

แขนซ้าย

1. ตั้งค่า UDP socket  : สร้างการเชื่อมต่อ UDP โดยใช้ที่อยู่ IP : localhost  Port : 12346 เพื่อสื่อสารกับ node-red
2.  ตั้งค่าอินเทอร์เฟซสำหรับ RTDE: สร้างการเชื่อมต่อทั้งการควบคุมและการรับข้อมูลกับโรบอท โดยใช้ที่อยู่ IP 192.168.3.45 เพื่อสื่อสารกับแขนกลโรบอท
3. ฟังก์ชันเคลื่อนที่แขนกล: กำหนดตำแหน่งให้โรบอทเคลื่อนที่ไปเตรียนหยิบแก้ว
4. ลูปรอเหตุการณ์: รอรับค่า “enter” แล้วทำงานต่อ
5. ฟังก์ชันเคลื่อนที่แขนกล: กำหนดตำแหน่งให้โรบอทเคลื่อนที่ไปจุดวางแก้ว

แขนขวา

1. ตั้งค่าอินเทอร์เฟซสำหรับ RTDE: สร้างการเชื่อมต่อทั้งการควบคุมและการรับข้อมูลกับโรบอท โดยใช้ที่อยู่ IP 192.168.3.46 เพื่อสื่อสารกับแขนกลโรบอท
2. ฟังก์ชันเคลื่อนที่แขนกล: กำหนดตำแหน่งให้โรบอทเคลื่อนที่ไปที่ตำแหน่ง home
3. รับค่าแกนโรบอท: นำค่าแกน โรบอทมา Forward kinematic เพื่อหาตำแหน่งของโรบอท
4. ฟังก์ชัน Vision : หาตำแหน่งของขวด แล้วนำมาแปรงเป็น transformation matrix ผ่าน พังก์ชัน camrobot
5.  พังก์ชัน invertkinematic : นำ transformation matrix มา invert kinematic เพื่อหาค่าแกนของโรบอท
6. ฟังก์ชันเคลื่อนที่แขนกล: ให้เคลื่อนที่ไปตำแหน่งที่กล้องหามาได้ด้วยคำสั่ง moveJ
7. ตั้งค่า UDP socket  : สร้างการเชื่อมต่อ UDP โดยใช้ที่อยู่ IP : localhost  Port : 12345 เพื่อสื่อสารกับ node-red
8. กำหนดค่าของฟังก์ชัน servoJ : velocity = 0.1 , acceleration = 0.1, dt = 2.0/1000, lookahead_time = 0.1, gain = 300
9. ลูปรอคำส่ง manual: รอรับค่าจาก node-red ผ่าน UDP ให้โรบอทเคลื่อนที่ไปยังทิศทางต่างๆ (ข้างหน้า, ข้างหลัง, ซ้าย, ขวา) โดยใช้คำสั่ง servoJ และรอรับคำสั่ง “enter” เพื่อทำงานต่อ
10. ฟังก์ชันเคลื่อนที่แขนกล: กำหนดตำแหน่งให้โรบอทเคลื่อนที่ไปที่ตำแหน่งวางขวด

สำหรับการทำ Program ของแขนขวา จะมี Function ของการทำ Manual Function เข้าไปด้วย

Program main

การรันโปรแกรมสองโปรแกรมพร้อมกันต้องใช้ module threading

1.กำหนดตัวแปร t1,t2 ให้เรียนใช้พันก์ชัน arm45,arm46

2.เริ่มรันโปรแกรมทั้งสองโปรแกรมโดยใช้คำสั่ง “.start()” ต่อหลังตัวแปร

Inverse kinematics Function

Ref : https://tianyusong.com/wp-content/uploads/2017/12/ur5_inverse_kinematics.pdf

Forward kinematics Function

Conclusion : จาก Program ที่ได้มีการเขียนขึ้นมาเพื่อ Control หุ่นยนต์ทั้ง 2 แขน สามารถใช้งานได้ตามเงื่อนไข และสำเร็จตาม Task ที่วางไว้

Progress :