Delta Robot Kinematics Library: ระบบควบคุมการเคลื่อนที่แม่นยำสูงสำหรับหุ่นยนต์แบบขนาน

ไลบรารีสำหรับคำนวณ Forward และ Inverse Kinematics ของหุ่นยนต์ Delta

ในโลกของหุ่นยนต์อุตสาหกรรม Delta Robot ถือเป็นดาวเด่นในกลุ่ม Parallel Robot เลยทีเดียว ด้วยความเร็วและความแม่นยำระดับหืดขึ้นคอ มันมักถูกใช้ในงานหยิบ-วาง (pick-and-place) บรรจุภัณฑ์ หรือแม้แต่การพิมพ์ 3D สิ่งที่ท้าทายที่สุดสำหรับหุ่นยนต์ประเภทนี้ไม่ใช่แค่โครงสร้างกลไก แต่เป็น "คณิตศาสตร์" ที่อยู่เบื้องหลังการเคลื่อนไหวต่างหาก

ไลบรารีนี้ถูกออกแบบมาเพื่อจัดการปัญหาทั้งสองด้านของ Kinematics นั่นคือ Forward Kinematics และ Inverse Kinematics ทำให้คุณสามารถควบคุมแขนกลของ Delta Robot ได้ง่ายดายเหมือนกับการสั่งงานด้วยพิกัดคาร์ทีเซียนทั่วไป

ขอบคุณแหล่งข้อมูล:

ขอขอบคุณแหล่งข้อมูลทางคณิตศาสตร์และวิศวกรรมที่เป็นพื้นฐานของไลบรารีนี้

การตั้งค่า Delta Kinematics

เพื่อให้ไลบรารีคำนวณตำแหน่งได้ถูกต้อง คุณต้องกำหนดขนาดทางกายภาพของหุ่นยนต์โดยใช้ตัวแปรหลัก 4 ตัว ตามที่แสดงในแผนภาพด้านล่าง:

คำอธิบายตัวแปรทางวิศวกรรม:

• ArmLength (L): ความยาวของแขนส่วนบน (Upper Arm) ที่ต่อตรงกับแกนมอเตอร์
• RodLength (l): ความยาวของแกนคู่ขนาน (Parallel Rods) ที่เชื่อมระหว่างแขนส่วนบนกับแท่น End-Effector
• BassTri (sb): ระยะห่างจากจุดศูนย์กลางของฐาน (Base) ไปยังจุดหมุนของมอเตอร์แต่ละตัว
• PlatformTri (sp): ระยะห่างจากจุดศูนย์กลางของแท่น End-Effector ไปยังจุดยึดของแขนแต่ละข้าง

คำแนะนำจัดหนัก: แม้ไลบรารีจะรองรับหน่วยวัดทั้งหมด (เมตร, นิ้ว) แต่ แนะนำให้ใช้ "มิลลิเมตร (mm)" เพื่อความละเอียดและความแม่นยำสูงสุดในการคำนวณตำแหน่งระดับไมโครนะตัวนี้

ฟังก์ชัน (Functions)

SETUP

DeltaKinematics(double ArmLength,double RodLength,double BassTri,double PlatformTri)

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

ฟังก์ชันนี้จะเซ็ตคลาสให้พร้อมคำนวณ Forward และ Inverse Kinematics สำหรับ Delta โดย ArmLength, RodLength, BassTri และ PlatformTri คือค่าที่วัดได้จากตัวหุ่นยนต์ Delta จริงๆ ดูรูปด้านบนประกอบจะเข้าใจว่าวัดยังไง

หน่วยที่ใช้กับ ArmLength, RodLength, BassTri และ PlatformTri ได้หมดเลย ไม่ว่าจะเป็น มิลลิเมตร, เมตร, นิ้ว หรือหน่วยความยาวอื่นๆ แต่อย่าลืม! ต้องใช้หน่วยเดียวกันทั้งแกน x, y และ z นะ

แนะนำให้ใช้หน่วยมิลลิเมตร (Millimetre) กับ ArmLength, RodLength, BassTri และ PlatformTri ครับพี่น้อง

SETUP

int forward()

int forward(double thetaA, double thetaB, double thetaC)

ใช้ฟังก์ชันนี้ตอนที่เรารู้มุมของมอเตอร์ทั้ง 3 ตัว (หน่วยเป็นองศา) แล้วอยากหาว่าตำแหน่งของหัวพิมพ์หรือ End-Effector ในพื้นที่ 3 มิติอยู่ตรงไหน

• การทำงาน: พอเราใส่ค่า thetaA, B, C เข้าไป ไลบรารีมันจะคำนวณหาจุดตัดของทรงกลม 3 ลูก (จุดที่แขนทั้งสามมาบรรจบกัน) แล้วเก็บผลลัพธ์ไว้ในตัวแปร x, y, z ของออบเจกต์

ฟังก์ชันนี้เอาไว้คำนวณ Forward Kinematics ของ Delta ครับ เราสามารถเซ็ตค่ามุมสำหรับคำนวณได้ผ่านตัวแปร a, b, c หรือจะส่งค่าเข้าไปในฟังก์ชันเป็น thetaA, thetaB, thetaC ก็ได้

ค่าของ thetaA, thetaB, thetaC หน่วยเป็นองศา ส่วน a, b, c ก็เป็นองศาเหมือนกัน (ดูรายละเอียดในหัวข้อ Variables ด้านล่าง) ดูรูปประกอบให้ดีนะ

int inverse()

int inverse(double x0, double y0, double z0)

นี่คือฟังก์ชันสำคัญสุดๆ สำหรับการควบคุมการเคลื่อนไหว (Motion Control) เมื่อเราอยากให้หุ่นยนต์เคลื่อนที่ไปยังตำแหน่งที่กำหนด (เช่น ไปที่ X=10, Y=20, Z=-150)

• การทำงาน: ระบบจะคำนวณย้อนกลับเพื่อหาว่ามอเตอร์แต่ละตัวต้องหมุนไปที่ "มุมกี่องศา" ถึงจะไปถึงตำแหน่งเป้าหมายนั้นได้ ผลลัพธ์จะถูกเก็บไว้ในตัวแปร a, b, c

ฟังก์ชันนี้เอาไว้คำนวณ Inverse Kinematics ของ Delta ครับ เราสามารถเซ็ตค่าตำแหน่งสำหรับคำนวณได้ผ่านตัวแปร x, y, z หรือจะส่งค่าเข้าไปในฟังก์ชันเป็น x0, y0, z0 ก็ได้

แนะนำให้ใช้หน่วยมิลลิเมตร (Millimetre) กับ x, y และ z

ตัวแปร (Variables)

double x, double y, double z

ตัวแปรเหล่านี้ใช้เก็บพิกัดในระบบแกนคาร์ทีเชียน (Cartesian)

• เมื่อเรียกใช้ forward() ผลลัพธ์จะถูกเก็บไว้ที่นี่
• เมื่อเรียกใช้ inverse(x0, y0, z0) ค่าเหล่านี้จะถูกใช้เป็นอินพุต
• หน่วย: แนะนำให้ใช้ มิลลิเมตร (mm)

ตัวแปร x, y และ z นี้สามารถอ่านและเขียนค่าได้ ใช้สำหรับกำหนดตำแหน่งของแท่น (Platform) หุ่นยนต์ Delta สำหรับการคำนวณ Kinematics ถ้าคำนวณ Forward Kinematics เสร็จ ค่าที่ได้จะถูกใส่ไว้ใน x, y และ z นี้แหละ

แนะนำให้ใช้หน่วยมิลลิเมตร (Millimetre) กับ x, y และ z ครับ ตัวนี้ห้ามช็อตนะ!

double a, double b, double c

ตัวแปรเหล่านี้ใช้เก็บ "มุมของมอเตอร์" (มุมของ Servo/Stepper)

• เมื่อเรียกใช้ inverse() ผลลัพธ์มุมที่คำนวณได้จะถูกเก็บไว้ที่นี่
• เมื่อเรียกใช้ forward(thetaA, thetaB, thetaC) ค่าเหล่านี้จะถูกใช้เป็นอินพุต
• หน่วย: องศา (Degrees)

ตัวแปร a, b และ c นี้สามารถอ่านและเขียนค่าได้ ใช้สำหรับกำหนดมุมของมอเตอร์หุ่นยนต์ Delta สำหรับการคำนวณ Kinematics ถ้าคำนวณ Inverse Kinematics เสร็จ ค่ามุมที่ได้จะถูกใส่ไว้ใน a, b และ c นี้

ค่าของ a, b และ c หน่วยเป็นองศา ดูรูปประกอบให้เคลียร์นะน้อง สู้งาน!

Deep Dive แบบช่างๆ: ทำไมต้องใช้ Kinematics Library?

ในโลกของวิศวะ การสั่งให้ Delta Robot เคลื่อนที่เนี่ย มันไม่ใช่แค่สั่งมอเตอร์ตัวนึงหมุนแล้วหวังให้ปลายจับวิ่งเป็นเส้นตรงหรอกนะตัวน้อง เพราะแขนทั้งสามมันส่งแรงที่เกี่ยวเนื่องกัน (Coupled Dynamics) ต้องคำนวณกันให้วุ่นเลย

1. Inverse Kinematics (จับปลายแล้วหาแขน) ของหุ่นยนต์ Delta เนี่ย หลักการมันคือการหาจุดตัดระหว่างวงกลมที่เกิดจากการกวาดของแขนบน กับทรงกลมที่เกิดจากความยาวของก้านล่างนั่นแหละ
2. Forward Kinematics (สั่งแขนแล้วหาปลาย) ยิ่งยากเข้าไปใหญ่ เพราะต้องแก้สมการพหุนามเพื่อหาจุดตัดของทรงกลมสามอัน (Trilateration) งานนี้ถ้าคิดเองอาจมึนได้

เจ้า Library ตัวนี้แหละที่จะจัดการความวุ่นวายของการคำนวณ Sine, Cosine, และ Square Root พวกนี้ให้อัตโนมัติ ลดภาระให้ CPU และประหยัดเวลาพัฒนาโปรแกรมควบคุมไปได้เยอะเลย สบายตัวน้อง จะได้โฟกัสกับลอจิกของโปรเจกต์เต็มที่ เช่น การหยิบจับวัตถุหรือการวาดรูป จัดไปวัยรุ่น!