FRA 631 Foundation-robotics: Automatic Robotic Arm System for Box Detection and Sorting

ที่มาและปัญหา

ปัจจุบันระบบอัตโนมัติและหุ่นยนต์อุตสาหกรรมมีบทบาทสำคัญในกระบวนการผลิต โดยเฉพาะในงานที่ต้องการ ความแม่นยำในการเคลื่อนที่, ความรวดเร็วในการหยิบจับวัตถุ และ ความปลอดภัยในการทำงานร่วมกับมนุษย์ หนึ่งในความท้าทายสำคัญคือการทำให้หุ่นยนต์หลายตัวสามารถ ประสานการทำงานร่วมกันได้อย่างราบรื่น โดยเฉพาะในกรณีที่ต้องมีการ ส่งต่อวัตถุระหว่างแขนกลสองตัว ซึ่งเป็นกระบวนการที่ซับซ้อนและต้องอาศัยการวางแผนเส้นทางและการควบคุมความเร็วอย่างแม่นยำ

            โครงการนี้มุ่งเน้นการพัฒนา ระบบจัดเรียงวัตถุด้วยแขนกล UR5e จำนวน 1 ตัว ที่ติดตั้งอยู่บนฐานกลางแบบสมมาตร โดยติดตั้งกล้อง Intel RealSense เพื่อใช้ในการระบุตำแหน่งวัตถุที่ไม่แน่นอนบนโต๊ะทำงาน พร้อมติดตั้ง Gripper แบบ 3 นิ้วมือ สำหรับหยิบวัตถุที่แขนกลตัวแรก จากนั้นทำการวางลงบนแขนกลอีกตัวที่ดัดแปลง Gripper เป็น แท่นรับวัตถุ (flat platform) เพื่อรองรับการรับวางวัตถุ

            ระบบควบคุมหุ่นยนต์ในโครงการนี้เขียนขึ้นใน Python โดยใช้คำสั่ง speedj ในการควบคุมการเคลื่อนที่ เพื่อควบคุมความเร็วของแต่ละข้อต่อในระดับ joint space โดยอิงจากการวางแผน trajectory ด้วย Cubic Polynomial ที่ให้ค่าตำแหน่งและความเร็วของปลายแขนในแต่ละช่วงเวลา หลังจากนั้นจึงแปลงข้อมูลเหล่านี้ให้เป็นความเร็วของข้อต่อผ่านกระบวนการทางคณิตศาสตร์ที่เกี่ยวข้องกับ Inverse Kinematics และ Jacobian  โครงการนี้ช่วยให้ผู้พัฒนามีโอกาสฝึกฝนการใช้งานหุ่นยนต์เชิงลึก ทั้งในด้านการเขียนโปรแกรมควบคุม การวางแผนเส้นทางการเคลื่อนที่ การใช้กล้องและเซนเซอร์ร่วมกับหุ่นยนต์ และการประยุกต์ใช้หลักการพื้นฐานของจลนศาสตร์หุ่นยนต์ ซึ่งสามารถนำไปต่อยอดในระบบอัตโนมัติจริงในภาคอุตสาหกรรม เช่น ระบบคลังสินค้าอัจฉริยะ การคัดแยกขยะอิเล็กทรอนิกส์ หรือการผลิตที่ใช้หุ่นยนต์ทำงานร่วมกัน

            นอกจากนี้ ระบบยังสามารถพัฒนาเพิ่มเติมในอนาคตให้รองรับการเรียนรู้และตัดสินใจแบบอัตโนมัติ โดยประยุกต์ใช้เทคนิค Machine Learning หรือ AI-Based Control เพื่อให้หุ่นยนต์สามารถรับรู้และปรับพฤติกรรมตามลักษณะวัตถุหรือสถานการณ์ที่เปลี่ยนแปลงได้ ซึ่งถือเป็นก้าวสำคัญของการพัฒนาไปสู่ Smart Factory และระบบอุตสาหกรรมในยุค Industry 4.0

วัตถุประสงค์

1. เพื่อศึกษาและออกแบบเส้นทางการเคลื่อนที่ของแขนกลสำหรับการหยิบวัตถุและส่งต่อวัตถุ
2. เพื่อศึกษาและออกแบบระบบควบคุมการเคลื่อนที่ของแขนกลโดยใช้ความเร็วในระดับ joint space ผ่านคำสั่ง speedj
3. เพื่อศึกษาและออกแบบกระบวนการทำงานในการวิเคราะห์การเรียงซ้อนกันของวัตถุและจัดเรียงใหม่

ขอบเขตการศึกษา

1. การศึกษาและออกแบบเส้นทางการเคลื่อนที่ของแขนกล ศึกษาและออกแบบเส้นทางการเคลื่อนที่โดยใช้หลักการของ Forward Kinematics และ Inverse Kinematics เพื่อคำนวณพิกัดของปลายแขนในแต่ละช่วงเวลา พร้อมทั้งนำแนวคิด Trajectory Planning มาใช้ในทั้ง Joint Space และ Cartesian Space เพื่อให้การเคลื่อนที่เป็นไปอย่างราบรื่นและปลอดภัย โดยมีการทดสอบและปรับค่าการเคลื่อนที่เพื่อหลีกเลี่ยงการชนกันระหว่างแขนกล (Collision Avoidance)
2. การควบคุมการเคลื่อนที่ของแขนกลด้วยความเร็วใน Joint Space ออกแบบระบบควบคุมโดยใช้ข้อมูลความเร็วของข้อต่อ (Joint Velocity) ที่ได้จากการวางแผน trajectory ใน Cartesian Space แล้วแปลงเป็นความเร็วของข้อต่อผ่าน Jacobian Matrix เพื่อนำไปสั่งงานด้วยคำสั่ง speedj โดยศึกษาการกำหนด Velocity, Acceleration และ Deceleration Profiles สำหรับแต่ละ joint และทดสอบผลกระทบของการควบคุมความเร็วต่อความแม่นยำในการส่งต่อวัตถุ
3. การประยุกต์ใช้กล้อง 3D ในการระบุตำแหน่งของวัตถุ ศึกษาการใช้งานกล้อง Intel RealSense 3D ที่ติดตั้งอยู่เหนือศูนย์กลางของพื้นที่ทำงาน เพื่อใช้ในการตรวจจับตำแหน่งของวัตถุบนโต๊ะ และแปลงค่าพิกัดจากกล้องให้อยู่ในระบบพิกัดโลก (World Coordinate) ของหุ่นยนต์ โดยข้อมูลตำแหน่งที่ได้จะถูกนำไปใช้ในการวางแผน trajectory สำหรับการหยิบวัตถุด้วยแขนกลตัวแรกอย่างแม่นยำ
4. การทดลองระบบในสภาพแวดล้อมควบคุมได้ ดำเนินการทดสอบระบบการส่งต่อวัตถุระหว่างหุ่นยนต์ UR5e ทั้งสองตัว โดยใช้วัตถุจำลองที่มีขนาดและน้ำหนักคงที่ ทำการเปรียบเทียบตำแหน่งที่คาดการณ์ไว้กับตำแหน่งจริงที่ได้จากการตรวจสอบ และวิเคราะห์ข้อผิดพลาดที่เกิดขึ้นเพื่อปรับปรุงความแม่นยำและเสถียรภาพของระบบ

ทฤษฏีที่เกี่ยวข้อง

Forward Kinematics (FK)

Forward Kinematics คือกระบวนการคำนวณ “ตำแหน่งและทิศทางของปลายแขนกล (End-Effector)” จากค่ามุมของข้อต่อแต่ละข้อ (Joint Angles) เมื่อเรากำหนดมุมของข้อต่อทุกข้อในหุ่นยนต์ เราสามารถใช้สมการทางคณิตศาสตร์ เช่น Denavit-Hartenberg (DH) Parameters เพื่อคำนวณว่า ปลายแขนกลจะอยู่ที่พิกัดใดในพื้นที่สามมิติ (3D Space) เช่น หุ่น UR5e ที่ใช้ในงานนี้

จะสามารถคิดเป็น transformation matrix ได้ด้วยวิธีนี้

หากคำนวณโดยไม่ใส่ค่า theta จะทำให้ผลลัพธ์ยาวมาก จึงนิยมใช้โปรแกรมคำนวณ เช่น matlab หรือ python

สมมุติให้ Joint Angles (deg):     [0, 0, 0, 0, 0, 0] จะได้ TCP Position in Base Frame:

หมายความว่า end effector ของหุ่นจะอยู่ที่ตำแหน่ง (-0.8172, -0.4329, 0.0628)

Inverse Kinematics (IK)

Inverse Kinematics คือกระบวนการหาค่ามุมของข้อต่อ (Joint Angles) ที่จะทำให้ปลายแขนกลไปอยู่ในตำแหน่งที่ต้องการ เมื่อเราระบุตำแหน่งปลายแขนกล เช่น (x, y, z) และทิศทางที่ต้องการ IK จะคำนวณว่า หุ่นต้องหมุนข้อต่อแต่ละข้อเป็นมุมเท่าใด จึงจะไปถึงจุดนั้นได้

ปัญหาคือ IK อาจจะมีหลายคำตอบ หรืออาจไม่มีคำตอบเลย เนื่องจากในบางตำแหน่งหุ่นสามารถเข้าถึงได้หลายท่า มีเทคนิคการคำนวณที่นิยมใช้ เช่น:

วิธีที่ 1: Analytical IK (การคำนวณเชิงวิเคราะห์) หลักการ ใช้ สมการคณิตศาสตร์ ตรีโกณมิติ และเมทริกซ์เพื่อหาคำตอบ มีข้อดีคือสามารถหาคำตอบที่เป็นไปได้ทุกท่า ข้อเสียคือหากหุ่นมีความซับซ้อนมาก จะเขียนโปรแกรมได้ยาก

วิธีที่ 2: Numerical IK (การคำนวณเชิงตัวเลข) หลักการ: ใช้ การประมาณค่า เช่น Damped Least Squares (DLS), Newton-Raphson, Jacobian inverse, pseudo-inverse โดยเริ่มจากค่าที่เดาไว้ก่อน (initial guess) แล้ววนรอบปรับค่ามุมไปเรื่อย ๆ จนตำแหน่งปลายแขนกลเข้าใกล้เป้าหมาย ข้อดีคือเขียนโปรแกรมคำนวณได้ง่ายกว่า แต่ข้อเสียคือจะหาผลลัพธ์ได้แค่ท่าเดียวซึ่งอาจจะไม่ใช่ท่าที่ต้องการ และไม่เหมาะกับการคำนวณเองในกระดาษเพราะต้องทำซ้ำหลายรอบจึงได้คำตอบ ในงานนี้เราใช้ วิธี Numerical IK เนื่องจากสะดวกในการเขียนโปรแกรม เพื่อลดความผิดพลาดในการเขียนโปรแกรม

ตัวอย่าง target_pos = [0.9104, 0.1506, 0.0713] ใช้วิธี Damped Least Squares (DLS) จะได้ Joint Angles (degree):   [0.3390   25.6795    0.0000 -115.0563 -270.0675  -90.3394] สามารถจำลองใน matlab ได้ดังนี้

Trajectory planning

Trajectory planning คือ การคำนวณชุดของตำแหน่ง ภายในทิศทางที่หุ่นยนต์จะเคลื่อนผ่านในช่วงเวลาใดเวลาหนึ่ง เพื่อให้ไปถึงเป้าหมายอย่าง “ปลอดภัย ราบรื่น และควบคุมได้” มีการคำนวณ 2 แบบ คือ Task space และ Joint space

ในงานนี้เลือกใช้วิธีการคำนวณใน task space เนื่องจากงานของเราคือ ให้หุ่นยนต์หยิบกล่องตามพิกัดที่ได้จากกล้อง ซึ่งกล้องส่งค่ามาใน โลกจริง (x, y, z) ไม่ใช่มุมข้อต่อ → เราจึงต้องใช้ Task Space Trajectory เพื่อเคลื่อนปลายแขนให้ตรงกับตำแหน่งนั้น

ตัวอย่าง หากต้องการเคลื่อนที่จาก target_pos1 = [0.4, -0.2, 0.2] ไป target_pos2 = [0.6, 0.2, 0.3] สมมุติให้แบ่งการเคลื่อนที่ออกเป็น 5 จุดตามวิธี cubic polynomial

การควบคุมหุ่นด้วย speed control ด้วย Jacobian ผ่านคำสั่ง speedj

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

ตัวอย่างแสดงการคำนวณ Jacobian และสั่ง speed control

ภาพรวมของระบบ (system overview)

ระบบที่พัฒนานี้เป็นระบบ หุ่นยนต์แขนกล UR5e แบบอัตโนมัติ สำหรับงาน ตรวจจับ, หยิบ, จัดเรียง และแยกกล่อง โดยใช้ 3D camera (Intel RealSense) เป็นตัวจับตำแหน่งของวัตถุภายใน workspace แล้วแปลงพิกัดจากภาพมาเป็นพิกัดหุ่นยนต์ผ่าน Python และ OpenCV ก่อนส่งคำสั่งควบคุมไปยังแขนกลผ่าน RTDE (Real-Time Data Exchange) API

แบ่งออกเป็น 2 ส่วนหลัก:

Work Space (พื้นที่ทำงาน)

• 3D Camera (Intel RealSense):
  ทำหน้าที่จับภาพและตรวจจับตำแหน่งวัตถุ (Object) ใน workspace
• Object:
  วัตถุหรือกล่องที่ต้องการตรวจจับ, หยิบ, และจัดเรียง
• UR5e Robot Arm & Gripper:
  แขนกลทำหน้าที่หยิบและจัดการกับวัตถุตามคำสั่งที่ได้รับจาก Controller
  • Gripper ใช้สำหรับจับวัตถุ
  • Robot Arm (UR5e) เคลื่อนที่ตาม trajectory ที่วางแผนไว้

Controller (ระบบควบคุม)

• Capture:
  ทำหน้าที่รับภาพจากกล้องและเตรียมข้อมูลภาพส่งให้ OpenCV
• OpenCV:
  ประมวลผลภาพและคำนวณพิกัด (x, y, z) ของวัตถุจาก ArUco marker หรือวิธีการตรวจจับอื่น ๆ
• Get image and x, y, z:
  ดึงข้อมูลภาพและตำแหน่งวัตถุเพื่อใช้ในการวางแผนการเคลื่อนที่ของแขนกล
• Kinematics Control & Trajectory Planning:
  • คำนวณ inverse kinematics
  • วางแผน trajectory สำหรับการเคลื่อนที่ไปยังตำแหน่งของวัตถุอย่างแม่นยำ
• Task Process:
  จัดการลำดับงานและตรวจสอบสถานะของแต่ละ task ในกระบวนการ (เช่น ตรวจสอบว่าหยิบเสร็จหรือยัง, วางเสร็จหรือไม่)
• RTDE Python:
  ส่งคำสั่ง (Send Command) ไปยัง UR5e และรับ feedback data กลับมาเพื่อตรวจสอบสถานะของการทำงาน

Feedback Loop

ระบบมี feedback loop ระหว่าง RTDE Python → Task Process → Kinematics Control & Trajectory Planning
เพื่อปรับ trajectory และควบคุมแขนกลแบบเรียลไทม์ พร้อมรับข้อมูลสถานะจากแขนกลตลอดเวลาเพื่อความแม่นยำและความปลอดภัย

จุดเด่นของระบบนี้

• ใช้กล้อง 3D สำหรับตรวจจับวัตถุใน workspace
• แปลงพิกัดจากกล้องเป็นพิกัดหุ่นยนต์แบบอัตโนมัติ
• รองรับ trajectory planning และ kinematics control ใน Python ด้วย OpenCV และ RTDE
• มี feedback loop เพื่อตรวจสอบและปรับ trajectory ได้อย่างต่อเนื่อง

ภาพรวมของระบบ (System Scenario)

ระบบที่พัฒนาขึ้นนี้ออกแบบมาเพื่อรองรับการทำงานของ หุ่นยนต์แขนกล UR5e สำหรับการหยิบและจัดเรียงกล่องโดยอาศัยข้อมูลพิกัดที่ได้จาก กล้อง 3D (Intel RealSense) และใช้ ArUco marker เป็นตัวช่วยในการคาลิเบรตและตรวจจับวัตถุ ระบบถูกแบ่งออกเป็น 3 ส่วนหลัก ได้แก่

1. Calibration Process
2. Task Process
3. Controller & Safety System

โดยมีวิศวกร 3 คน ทำหน้าที่ควบคุมและดูแลในแต่ละส่วน เพื่อให้ระบบทำงานได้อย่างถูกต้องและปลอดภัย

Calibration Process (กระบวนการคาลิเบรต)

• Engineer 2 ทำหน้าที่ ติดตั้ง ArUco marker ใน workspace เพื่อเป็นจุดอ้างอิงสำหรับการคาลิเบรต
• กล้อง 3D ตรวจจับตำแหน่งของ marker และส่งข้อมูลไปที่ Calibration Method ซึ่งใช้ OpenCV เพื่อคำนวณ Transformation Matrix
• ผลลัพธ์จะถูกแสดงใน Transform Results และนำไปใช้ใน Controller เพื่อแปลงพิกัดจากกล้องเป็นพิกัดของหุ่นยนต์

Task Process (กระบวนการหยิบจับและจัดเรียง)

• หลังจากคาลิเบรตเสร็จสิ้น Engineer 2 เปลี่ยน workspace ให้พร้อมสำหรับการปฏิบัติงานจริง (วางวัตถุหรือกล่องที่ต้องจัดการ)
• กล้อง 3D ตรวจจับตำแหน่งของกล่องแบบเรียลไทม์
• ข้อมูลตำแหน่งจะถูกส่งไปที่ Controller เพื่อวางแผน trajectory และคำนวณ inverse kinematics
• หุ่นยนต์ UR5e ทำการ หยิบ, วาง, จัดเรียง และแยกกล่อง ตามคำสั่งที่ได้วางแผนไว้

Controller & Safety System (ระบบควบคุมและความปลอดภัย)

• Engineer 1 เป็นผู้ควบคุมการทำงานของระบบ สามารถเลือกได้ว่าจะ ควบคุมเอง (Take control) หรือ รับ Feedback จากระบบ (Get Feedback)
• Controller ใช้ OpenCV สำหรับแปลงพิกัดจากกล้องเป็นพิกัดหุ่นยนต์ และใช้ RTDE Python API ในการส่งคำสั่งควบคุมแขนกล
• ขณะปฏิบัติงาน ระบบจะตรวจสอบ Error while process ตลอดเวลา
• หากพบปัญหาหรือเหตุขัดข้อง Engineer 3 มีหน้าที่ดูแลด้านความปลอดภัยและสามารถกด Emergency Stop (E-stop) เพื่อหยุดกระบวนการทันที

วงจรการทำงานของระบบ

1. เริ่มคาลิเบรต → คำนวณ Transformation Matrix
2. ตั้งค่าพื้นที่ทำงานสำหรับหยิบวัตถุ
3. จับภาพ ตรวจจับวัตถุ คำนวณ trajectory
4. ควบคุมแขนกลเพื่อหยิบและวางวัตถุ
5. ตรวจสอบ error และ feedback ตลอดการทำงาน
6. หยุดการทำงานหากพบความผิดปกติเพื่อความปลอดภัย

ภาพรวมของระบบ (Hardware Overview)

1. แขนกล UR5e
2. Robotiq 3 finger gripper (ติดปลายแขนตัวที่ 1)
3. Intel Realsense 3D Camera ( d435 as global camera )
4. คอมพิวเตอร์สำหรับประมวลผลและสั่งการ
5. กล่องลูกบาศก์ขนาด 5*5*5 ลบ.ซม. จำนวน 4 กล่อง
6. โต๊ะทำงานขนาด 100*100 ตร.ซม.

การนำเสนอผลการออกแบบ (Design)

Tool Calibration

• มีการสร้างอุปกรณ์ขึ้นมาใหม่สำหรับให้แขนกลจับ Aruco marker ได้ง่ายขึ้น และปลอดภัยขึ้น (เนื่องจากข้อต่อข้อ J4 ยื่นลงด้านล่างและชนกับโต๊ะทำงานได้ง่าย จนไม่สามารถยื่นปลายแขนหุ่นลงไปเก็บค่าตำแหน่งบนพื้นโต๊ะได้) ลักษณะเป็นแท่นสีเหลี่ยมความยาว 18 ซม. และมีปลายยื่นไปด้านหน้า 18 ซม.

การออกแบบทางโปรแกรม (Design Programing System)

การ Calibration

หุ่นยนต์จะทำงานตามคำสั่งโดยอิงจากการสังเกตของกล้อง การปรับเทียบ (Calibration) ทำหน้าที่ปรับแนวแกนพิกัดของหุ่นยนต์และกล้องให้ตรงกัน โดยใช้การแปลงเมทริกซ์แบบ Affine Matrix Transformation เหตุผลที่ใช้ Affine matrix เนื่องจากสามารถจัดการได้ทั้งการเลื่อนตำแหน่ง หมุน และปรับขนาด ซึ่งทั้งหมดนี้สำคัญมากเวลาเราต้องทำให้พิกัดของกล้องกับพิกัดของแขนหุ่นยนต์ตรงกัน

Calibration Steps

• กำหนดพื้นที่ทำงานแบบ 2 มิติ: วาง Aruco markers ไว้ที่มุมทั้งสี่ของพื้นที่
• ขยายเป็นพื้นที่ 3 มิติ: สร้างพื้นที่สามมิติโดยการวางระนาบหลายชั้นซ้อนกัน
• เลือกจุดตรวจวัด (Waypoint): กำหนดตำแหน่งจุดตรวจวัดทั้งหมด 27 จุด
• เก็บข้อมูล: หุ่นยนต์และกล้องจะบันทึกตำแหน่งที่แต่ละจุดตรวจวัด เพื่อใช้ในการปรับแก้

• หลังจากเก็บข้อมูลตำแหน่งจากกล้องและหุ่นมาได้จะทำคำนวณ Affine Matrix โดยใช้หลักการ Numerical method เพื่อหา RMS matrix ในการแปลงค่าจากกล้องไปเป็นหุ่นให้มี error น้อยที่สุด

• มีการสร้างอุปกรณ์ขึ้นมาใหม่สำหรับให้แขนกลจับ Aruco marker ได้ง่ายขึ้น และปลอดภัยขึ้น (เนื่องจากข้อต่อข้อ J4 ยื่นลงด้านล่างและชนกับโต๊ะทำงานได้ง่าย จนไม่สามารถยื่นปลายแขนหุ่นลงไปเก็บค่าตำแหน่งบนพื้นโต๊ะได้) ลักษณะเป็นแท่นสีเหลี่ยมความยาว 18 ซม. และมีปลายยื่นไปด้านหน้า 18 ซม.

การตั้งค่าตัวแปรสำหรับอ้างอิงในการปรับแก้ (main.py)

• เมื่อเริ่มเก็บข้อมูลโดยใช้ python3 main.py หุ่นจะขยับไปตามตำแหน่งต่างๆใน waypoint และเก็บค่าไว้ใน calibration_data.csv

• หลังจากได้ข้อมูลแล้วก็มารัน python3 caribration-affine_matrix.py เพื่อหาเมทริกซ์ที่ error น้อยที่สุด best_matrix.json

จากการทำซ้ำ 20,000 รอบ ก็ได้

Best RMS Error: 0.004 (X: 0.002, Y: 0.008, Z: 0.003) เมทริกซ์นี้จะถูกเซฟไว้ใช้สำหรับปรับแก้ค่าที่อ่านได้จากกล้องเพื่อใช้ในการสั่งการหุ่น

การสั่งการหุ่นยนต์โดยใช้ Speed control, Trajectory planning

ในการสั่ง speed control จำเป็นต้องมีการคิด trajectory planning ก่อน โดยการคิด trajectory planning มีวิธีคิด 2 แบบ

• Task space จะใช้การกำหนดจุดเริ่มต้นและเส้นทางไปสู่จุดปลายก่อนแล้วแบ่งซอยออกเป็นจุดย่อยๆระหว่างทาง จากนั้นจึงใช้ IK ในการคำนวณท่าทางของหุ่นในแต่ละจุดย่อยๆนั้น จากนั้นจึงค่อยเอาค่าที่คิดได้ไปใช้สั่งการเคลื่อนที่ของหุ่นอีกที ข้อดีคือจะได้ท่าทางที่แม่นยำ แต่ข้อเสียคือต้องใช้เวลาคำนวณนานเพราะต้องคิด IK ใหม่ทุกครั้งที่ขยับ
• Joint space จะใช้การกำหนดจุดเริ่มต้นและจุดปลายก่อนเหมือนกับ task space แต่จะใช้ IK คำนวณท่าทางของหุ่นในทั้ง 2 จุด จากนั้นจึงวางแผนการเคลื่อนที่โดยการปรับมุมข้อต่อของหุ่นจากท่าจุดเริ่มต้นไปท่าจุดปลาย มีการแบ่งซอยช่วงการเคลื่อนที่ออกเป็นช่วงย่อยๆเหมือนกัน ข้อดีคือใช้เวลาคำนวณน้อยกว่า แต่ข้อเสียคือแม่นยำน้อยกว่าเช่นกัน
• ในงานนี้จะเลือกใช้การคำนวณแบบ task space เนื่องจากต้องการให้สามารถควบคุมได้อย่างแม่นยำมากกว่าความไว
• ทางทีมงานได้เลือกใช้วิธีคำนวณแบบ Cubic polynomial (3rd Order) เพื่อให้การเคลื่อนที่มีความลื่นไหล

• เริ่มจากการกำหนดตำแแหน่ง และความเร็วของ จุดเริ่มและปลาย จากนั้นจึงคำนวณหาเวลาทั้งหมดที่ต้องใช้ในการเคลื่อนที่ (T_total) โดยเลือกว่าจะให้ velocity profile เป็นแบบ Triangular หรือ Trapezoidal กล่าวคือ Triangular นั้นเป้นการสั่งแบบให้เร่งและชลอโดยไม่มีการคงความเร็วไว้คงที่เหมาะกับการสั่งในระยะทางสั้นๆ แต่แบบ Trapezoidal จะมีการรักษาความเร็วคงที่ระหว่างทางไว้ด้วย เหมาะกับระยะทางที่ยาว

• เมื่อได้ T_total มาแล้วก็นำไปคำนวณเพื่อ ช่วงเวลาของแต่ละ step ย่อย สุดท้ายจะได้ความเร็ว ความเร่งและตำแหน่งของแต่ละช่วง

• เริ่มแรกหา T_total จาก inpur v_tool a_max และ distance
  

• หลังจากได้ waypoint ใน task space แล้วเราก็ใช้ IK เพื่อเปลี่ยนมันเป็น joint space

• ในการสั่ง speed control แบบ real time จะทำผ่าน Jacobian matrix เพื่อแปลงจาก end effector speed เป็น joint speed

• เมื่อคำนวณ Trajectory เสร็จแล้วจะใช้ค่า Joint current state มาคำนวณ Jacobian matrix และ Invert Jacobian
• นำค่า End effector speed มาคูณกับ Invert Jacobian จะสามารถแปลงเป็น Joint velocity เพื่อใช้สั่งการหุ่นด้วย speedj โดย loop การคำนวณจะทำซ้ำเมื่อขยับตำแหน่งใหม่ไปเรื่อยๆจนถึงเป้าหมาย จึงสั่งให้หยุด

Task Planning

ใช้ RealSense depth-camera ตรวจจับ ArUco marker บน “กล่อง” (ID < 100) และ ตำแหน่งว่าง (ID ≥ 100) แล้วคำนวณพิกัด 3-มิติด้วย point-cloud

ค่า STACK_Y_THRESHOLD = 0.25 m (≈ 0.3 m ในไดอะแกรม) thereshold มาจากการใช้ position ที่ถ่ายได้จากกล้อง x affine matrix จากการ calibration เป็นตัวแบ่งว่ากล่องใดถือว่า มีกล่องอยู่ข้างบน แบ่งออกเป้น 3 phase

Phase 1 – Destack (คลายกองเฉพาะโซนซ้ายหรือขวา)

• detect_overlaps() กรองเฉพาะกล่องจริง (id < 100)
  • ถ้า Y < 0.25 m → ถือว่ามีกล่องซ้อน (stacked)
• destack_within_zone(group_ids) ทำงานวนลูปจนไม่เหลือ stack ในโซนที่เลือก
  1. หากยัง stacked → min_marker() หา กล่องที่ต่ำสุด
  2. หา empty-spot ในกลุ่ม (100-102 หรือ 104-106) ที่ยังไม่ได้ใช้
  3. pick_box() ยกกล่อง → place_box() วางบนตำแหน่งว่าง
  4. บันทึก ID ตำแหน่งว่างนั้นใน used_empty_markers เพื่อไม่ใช้ซ้ำ

Phase 2 – Arrange / Stack

เมื่อไม่พบ stack แล้ว หรือผู้ใช้ข้ามขั้น (เลือก “Custom”)

• สร้างรายชื่อ
  • box_ids = กล่องทั้งหมด
  • empty_ids = ตำแหน่งว่างทั้งหมด
• ผู้ใช้เลือกวิธีวาง
  1. น้อย→มาก (min→max)
  2. มาก→น้อย (max→min)
  3. กำหนดเอง (พิมพ์ลำดับ ID)
• สำหรับกล่องแต่ละใบใน seq
  1. pick_box() ยกกล่อง
  2. ถ่ายภาพใหม่หาตำแหน่ง marker ปลายทาง dest
  3. place_box() วางกล่อง
  4. ตั้ง dest = bid เพื่อวางกล่องถัดไปทับกล่องนี้ → เกิดกองเดียวสูงขึ้นเรื่อย ๆ

Phase 3 – Handover (ถ้าเลือกปลายทาง ID 200)

• หุ่นยนต์ “Gripper” ขยับไปท่าถ่ายโอน (GRIPPER_HAND_OVER_POS)
• หุ่น “Hand Robot” (robot_hand) ไปที่ HAND_OVER_POS รอรับ
• วนลูปทีละกล่อง
  1. Gripper ยกกล่อง → ไปจุด handover
  2. ปล่อยลงบน marker 200 บนมืออีกหุ่น
  3. Hand-Robot ไปท่าปล่อย (hand_drop_pos) แล้วกลับ safety-home
• จบงาน: เรียก stop_all() ปล่อยเซสชันหุ่น/กล้อง

แผนการดำเนินการ

เป้าหมายสัปดาห์ที่ 1 (17 – 23 มีนาคม 2568): วิเคราะห์ปัญหาและออกแบบระบบ

• กำหนดวัตถุประสงค์
• ร่างผังระบบหุ่นยนต์ 2 แขน
• กำหนดตำแหน่งติดตั้งกล้อง
• นิยามขอบเขตและรูปแบบการรับส่งวัตถุ

เป้าหมายสัปดาห์ที่ 2 (24 – 30 มีนาคม 2568): ติดตั้งและทดสอบอุปกรณ์

• ติดตั้งกล้อง Intel RealSense
• กำหนดพิกัด world frame
• ทดสอบอ่านข้อมูล RGB + depth
• ตรวจสอบตำแหน่งวัตถุจากกล้อง

เป้าหมายสัปดาห์ที่ 3 (31 มีนาคม – 6 เมษายน 2568): สร้างระบบแปลงพิกัดกล้อง → หุ่นยนต์

• แปลงพิกัดจากกล้องเป็นพิกัด world
• ทดสอบความแม่นยำของพิกัด
• สร้างฟังก์ชันอ่านตำแหน่งวัตถุแบบ real-time

เป้าหมายสัปดาห์ที่ 4 (7 – 13 เมษายน 2568): ออกแบบ Trajectory Planning

• วางเส้นทางปลายแขน (task space)
• สร้าง cubic polynomial หรือ spline
• คำนวณตำแหน่งและความเร็วของปลายแขน

เป้าหมายสัปดาห์ที่ 5 (14 – 20 เมษายน 2568): แปลงความเร็วไปเป็น joint space

• – เขียนฟังก์ชันคำนวณ Inverse Kinematics
• – สร้าง Jacobian และแปลง x˙→q˙
• – สั่งหุ่นยนต์ด้วย speedj(...)

เป้าหมายสัปดาห์ที่ 6 (21 – 27 เมษายน 2568): ทดสอบการหยิบและส่งวัตถุ

• หยิบวัตถุด้วยแขนกลซ้าย
• ส่งต่อไปยัง platform ของแขนขวา
• ปรับ trajectory และความเร็วให้เหมาะสม

เป้าหมายสัปดาห์ที่ 7 (28 เมษายน – 4 พฤษภาคม 2568): ทดสอบซ้ำ + วิเคราะห์ข้อผิดพลาด

• ตรวจสอบความแม่นยำของการส่งต่อ
• เปรียบเทียบพิกัดจริง vs พิกัดเป้าหมาย
• วิเคราะห์และลด error

เป้าหมายสัปดาห์ที่ 8 (5 – 9 พฤษภาคม 2568): สรุปผลและจัดทำรายงาน

• สรุปผลการทดลอง
• จัดทำเอกสารรายงานฉบับสมบูรณ์
• เตรียมนำเสนอ/ทดสอบระบบเต็มรูปแบบ

การทดสอบระบบ

มีการทดสอบระบบทั้งหมด 6 หัวข้อ
1. ความแม่นยำของการหยิบจับ (Pick Accuracy) ทดสอบ: หยิบกล่องในตำแหน่งเดิมซ้ำๆ โดยไม่ใช้พิกัดจากกล้อง (10 ครั้ง) เพื่อวิเคราะห์ว่า หยิบพลาดกี่ครั้ง
2. ความแม่นยำของการวาง (Place Accuracy) ทดสอบ: วางกล่องลงบนแท่น arm2 ซ้ำๆ (10 ครั้ง) เพื่อวิเคราะห์ว่า วางพลาดกี่ครั้ง
3. ความเสถียรของระบบตรวจจับภาพ (Vision Robustness) ทดสอบ: วางกล่องที่ตำแหน่งและมุมต่างกัน (10 ครั้ง) เพื่อวิเคราะห์ว่า ArUco marker ตรวจเจอทุกครั้งหรือไม่
4. ความถูกต้องของการแปลงพิกัดกล้องเป็น world coordinate ทดสอบ: ใช้ object วัดตำแหน่งจริง แล้วเปรียบเทียบกับพิกัดที่กล้องให้ (10 ครั้ง) เพื่อวิเคราะห์ ความคลาดเคลื่อนของตำแหน่ง (mm)
5. ความต่อเนื่องในการส่งของ (Task Completion Rate) ทดสอบ: สั่งการ task เดิม ซ้ำๆ (10 ครั้ง) เพื่อวิเคราะห์ สำเร็จกี่รอบ
6. ความผิดพลาดของ Inverse Kinematics / Trajectory ทดสอบ: ลองเป้าหมายใน workspace หลายจุด (10 ครั้ง) เพื่อวิเคราะห์ว่า มีตำแหน่งไหนที่ IK หาไม่ได้หรือ trajectory คำนวณแล้วพุ่งผิดทางไหม

ตารางบันทึกผลการทดสอบ

สรุปผลการทดสอบ

จากการทดสอบทั้งหมดพบว่า ระบบสามารถทำงานได้ตาม task ที่สั่งสำเร็จ 97% ข้อผิดพลาดส่วนใหญ่มาจากการแปลงพิกัดจากกล้อง เป็น world เนื่องจากในคู่มีของกล้องมีการเขียนอธิบายอยู่ว่ากล้อง intel realsense d435i จะมีความคลาดเคลื่อนในการอ่านทีกัดอยู่ที่ >2% ที่ระยะไม่เกิน 2 m (คิดเป็น +- 4 ซม) ถึงแม้จะไม่ใช้ระยะทีมากแต่ก็ยังส่งผลต่องานของเราเนื่องจากวัตถุที่ใช้หยิบมีขนาด 5*5*5 ลบ.ซม. ทำให้ทุกครั้งที่หยิบถึงแม้จะหยิบได้ แต่ตำแหน่งจะเปลี่ยนไปเล็กน้อยเสมอ
การคำนวณ IK และ Trajectory จะไม่มีปัญหาหากทำภายในพื้นที่ๆเราทดสอบแล้ว และไม่เปลี่ยน orientation ของ gripper ทุกครั้งที่จะเปลี่ยน orientation จึงจำเป็นจะต้องใช้คำสั่ง movel แทน เพื่อลดความผิดพลาด
ในส่วนของการสั่งการผ่านคำสั่ง speedj พบว่าควรจะมีการทำ feedback loop เพื่อชดเชยค่าที่คลาดเคลื่อนจากภายในตัวหุ่นเอง

ข้อเสนอแนะและจุดสังเกต

• ในครั้งหน้าควรมีการปรับเปลี่ยนลักษณะงานให้เหมาะสมกับความละเอียดของกล้องให้มากกว่านี้
• การทดลองครั้งนี้มีการเปลี่ยนกล้องในระหว่างการทดลองจาก intel realsense d435i เป็น d455 ทำให้ค่าที่อ่านได้จากกล้องมีความคลาดเคลื่อน ถึงแม้กล้องทั้งสองจะมีประสิทธิภาพสูงทั้งคู่ แต่การเปลี่ยนกล้องระหว่างการทดลองทำให้ต้องทำการ calibrate ใหม่ ส่งผลให้การทำงานล่าช้า
• การยึดเสากล้องไว้กับเสาที่ใช้ยึดหุ่นยนต์ถึง 2 ตัว ทำให้เวลาหุ่นยนต์ขยับกล้องจะสั่น ส่งผลให้เกิดความคลาดเคลื่อนในบางครั้ง ในอนาคตควรจะแยกจุดยึดกล้องออกจากเสาที่ใช้ยึดเกาะหุ่น
• ในครั้งหน้าควรมีการใช้ feedback loop กับการเปลี่ยน orientation ด้วย
• ในครั้งหน้าควรจะออกแบบให้มีการคำนวณ orientation ของกล่อง เพื่อปรับท่าทางการเข้าจับของหุ่นให้ดีขึ้น
• การ calibrate ที่ใช้ในครั้งนี้เหมาะกับการใช้งานระหว่าง กล้อง 1 ตัว กับหุ่นไม่เกิน 2 ตัว ที่ไม่ต้องทำงานร่วมกันอย่างละเอียดมากเนื่องจากเป็นการแปลงค่าจากกล้องเป็นฐานของหุ่นตัวแรก ในการสั่งการหุ่นตัวที่ 2 จำเป็นจะต้องแปลงค่าจะฐานหุ่นตัวแรกป็นฐานหุ่นตัวที่สองอีกที ซึ่งในกรณีนี้ยังไม่ถือว่าซับซ้อนมาก แต่หากมีหุ่นยนต์ 3 ตัวขึ้นไปทำงานร่วมกัน หรือมีกล้องมากกว่า 1 ตัว การแปลงพิกัดจะเกิดความยุ่งยากกว่าการใช้ transformation matrix ในการแปลงค่าจากกล้องมาเป็น world frame แล้วค่อยแปลงกลับเข้าสู่ฐานของหุ่นแต่ละตัว

ภาพถ่ายและการทำงานของระบบ

สมาชิกผู้จัดทำ

1. นาย ศิวรุตม์ จุลพรหม (ทำหน้าที่เป็นผู้คำนวณค่าต่างๆเพื่อใช้ตรวจสอบความสอดคล้องและถูกต้องระหว่างค่าจะการทดลองจริงกับทฤษฎี วิเคราะห์และหาทางแก้ไขความผิดพลาดต่างๆที่เกิดขึ้นในระหว่างทดลอง)
2. นาย พงษ์พัฒน์ วงศ์กำแหงหาญ (ที่หน้าที่เป็นผู้ออกแบบและพัฒนา task และเขียนแบบ 3 มิติของชิ้นงานที่สร้างขึ้นมาใหม่)
3. นาย พีรดนย์ เรืองแก้ว (ทำหน้าที่เป็นผู้ทดสอบและปรับแก้ค่าต่างๆ ไปจนถึงสร้างคำสั่งพื้นฐานในการควบคุมหุ่นยนต์)

เอกสารอ้างอิง

https://www.youtube.com/watch?v=Fd7wjZDoh7g

https://www.linearmotiontips.com/how-to-calculate-velocity/

https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics

Source Code

https://github.com/peeradonmoke2002/FRA631_Project_arm_UR5e.git