2 Hand Avatar Robot : Kinematics – Human-Computer Interface Lab

Main Page : https://hcilab.net/class-project/2-hand-avatar-robot/

Kinematics Calculation

ในโปรเจท 2 Hands Avatar Robot : Robot ที่ทางกลุ่มเลือกใช้จะเป็น UR5 2 ตัวทำงานร่วมกัน ดังนั้นการที่จะสามารถควบคุม Robot ทั้ง 2 แขน ได้จะต้องคำนวน Forward Kinematics และ Inverse Kinematics ก่อนเป็นอย่างแรก

Forward Kinematics

เริ่มจาก Specification Sheet ของ Robot UR5

DH – Transformation

เริ่มจากนำ Diagram ของ Robot UR5 ตั้งแกนตามกฎของ DH – Transformation ทั้ง 6 แกน จะได้แกนและตัวแปร a, d, alpha, theta ดังนี้

เมื่อนำค่าที่ได้ มาคำนวน Forward Kinematics จากสูตร DH – Transformation จะได้

ทำการเช็คว่า ที่ตั้งแกนตาม DH-Transformation และตัวแปรถูกต้องไหม โดยการเช็คกับ Program RoboAnalyzer – โดยกำหนด theta ที่ตรงกันแล้ว เทียบค่า T⁰₆ ที่คำนวนกับ Link 6 เทียบกับ Base Frame ใน Program RoboAnalyzer ==> θ1 = 0°, θ2 = -90°, θ3 = 0°, θ4 = -90°, θ5 = 0°, θ6 = 0°

Transformation Matrix จาก Program RoboAnalyzer

ค่าจากทั้ง 2 ที่มา มีค่าเท่ากัน เพราะฉะนั้น DH – Transformation ที่คำนวนมากถูกต้อง

Inverse Kinematics

จาก Paper “UR5 Inverse Kinematics” โดย Ryan Keating และ Noah J. Cowan จาก Johns Hopkins University

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

เราจะสามารถหาคำตอบของ θ1, θ2, θ3, θ4, θ5 และ θ6 ได้ดังนี้

โดย T⁰₆ เป็น Transformation Matrix ของ Frame ที่ 6 (ไม่รวม Gripper) เทียบกับ Base Frame ซึ่งก็คือเป้าหมายที่ Robot

หลังจากคำนวนหา θ1 ได้แล้วจะสามารถคำนวนหา θ5 ได้จากสมการข้างบน

จาก DH – Transformation ก่อนหน้า ทำให้เราสามารถคำนวนหา T⁰₁ และได้ T⁶₁ ออกมา จากนั้นนำค่า X , Y ไปคำนวนหา θ6

หลังจากการคำนวนหา θ6 ทำให้เราคำนวนหา T¹₄ ได้และคำนวนหา θ3, θ2

หลังจากการคำนวนหา θ3, θ2 จะเหลือ θ4 โดยคำนวนจากการหาค่าแถวแรกของ T³₄ ตามสมการด้านบน

Path Planning

ผลลัพธ์ที่ได้จะเห็นว่ามี คำตอบของสมการ Inverse Kinematics ทั้งหมด 8 คำตอบ จึงคิดหาวิธีการทำ Path Planning ให้ระบบเลือก Path ที่สั้นที่สุด โดยการ

หาผลต่างของแกนทั้ง 6 แกนของทั้ง 8 คำตอบ จากตำแหน่งแกนปัจจุบัน
รวมผลต่างของแต่ละคำตอบ
เลือกคำตอบที่มีผลรวมของผลต่างน้อยที่สุด

ทำการเช็คคำตอบของ Inverse Kinematics และ Path Planning ว่าถูกต้องหรือไม่ โดยการนำ ตำแหน่งแกน θ1, θ2, θ3, θ4, θ5 และ θ6 ตัวอย่างไปเข้า ฟังก์ชัน Forward Kinematics แล้วนำ Transformation Matrix ที่ได้มาเข้า ฟังก์ชัน Inverse Kinematics และ Path Planning ดูเพื่อเช็ค ค่าเริ่มต้น และ ค่าที่ได้จากฟังก์ชันมา เทียบกัน

โดยค่าเริ่มต้นของทั้ง 6 แกน ได้แก่ [1.048, -1.513, 1.869, -0.420, 1.833, 3.063]

ผลลัพธ์ที่ได้จะเห็นว่า จากค่าเริ่มต้นเทียบกับค่าที่ออกมาจากฟังก์ชันนั้นมีค่าใกล้เคียงกัน มีผลต่างเล็กน้อย เนื่องจากการปัดเศษของทศนิยม เพราะฉะนั้น สมการและ Code ที่เขียนขึ้นมานั้นสามารถใช้งานได้และถูกต้อง

Jacobian and Singularity

เนื่องจากฟังก์ชัน UR_RTDE ที่ใช้ควบคุม Robot ของเรานั้นไม่สามารถเปลี่ยนความเร็วตามแต่ละช่วงเวลาได้ ทางทีมจึงต้อง Fix ความเร็วของ Robot ไว้เป็นค่าคงที่ แต่ก็ยังสามารถใช้ประโยชน์จาก Jacobian ได้ด้วยการป้องกันการเกิด Singularity ด้วยการหา Det ของ Jacobian Matrix

โดยเราสามารถคำนวนหา Jacobian Matrix จาก สามารถตามรูปด้านซ้ายนี้ (UR5 Joint ทั้งหมดเป็น Revolute Joint)

SingularityCheck(joint) = ฟังก์ชันเช็ค Singularity ด้วยการใส่ค่าของแต่ละ Joint แล้วจะ return ค่า Det ของ Jacobian มา

เริ่มต้นด้วย กำหนด D-H Parameter ตาม Forward Kinematics ก่อนหน้านี้
หา D-H Transformations ของแต่ละคู่ ด้วยฟังก์ชัน cal(a,alpha,d,q) ที่เขียนมาก่อนหน้าตอนคำนวน Forward Kinematics

คำนวนหา Matrix ที่จำเป็นสำหรับ คำนวน Jacobian Matrix

คำนวนหา Jacobian Matrix (J1 – J6) จากนั้น รวม J1 – J6 เป็น Jacobian Matrix ของ UR5 แล้ว **return Det ของ Jacobian Matrix** กลับมา

โดยเราจะนำฟังก์ชัน SigularityCheck(joint) นี้มาเช็คตอนนี้เราสั่งโรบอท แบบ Manual ผ่านทาง VR : ในทุกๆครั้งที่ User สั่ง Robot เคลื่อนไหว SigularityCheck(joint) ห้ามมีค่า +- 0.0001 ไม่ฉะนั้นจะไม่สามารถเคลื่อนที่ต่อไปได้ เพื่อป้องกันความเสียหายที่จะเกิดขึ้นกับ Robot

Camera to Robot Kinematics

ในส่วนของทีม Vision Detect ขวดน้ำ เมื่อทางทีมสามารถ ระบุตำแหน่งของขวดน้ำได้แล้ว ก็จะต้องทำการส่งค่ามาให้ Robot สามารถรับรู้ตำแหน่งของขวดน้ำได้ โดยทำการใช้ Homogeneous Transformation จาก Robot ไปยัง Frame ของกล้องและ Frame ของขวดน้ำ โดยเราให้ World Frame อยู่ที่ Base Frame ของ Robot ฝั่งขวา เพื่อให้ง่ายต่อการคำนวน

จากรูปจะเห็นว่า แกนของ Base Frame ยังไม่อยู่แนวเดียวกับ Camara Frame จึงต้อง

หมุนแกน Z = -45°
เลื่อนแกน Y = -0.83 m , เลื่อนแกน Z = 0.10 m, เลื่อนแกน X = -0.19 m
หมุนแกน Y = -90°

จะได้สมการตามรูปด้านขวา

จาก Camara Frame ไปยัง Target Frame นั้นสามารถทำได้โดย

เลื่อนแกน X = X ที่กล้อง Detect ได้ , เลื่อนแกน Y = Y ที่กล้อง Detect ได้
เลื่อนแกน X = 0.320 m , เลื่อนแกน Z = -0.195 m

โดยเลื่อน X ครั้งสุดท้ายคือ เลื่อนออกเผื่อระยะของ Gripper และ เลื่อน Z คือ Fix ความสูงของ Gripper ที่จะเข้าไปจับขวดน้ำ

สุดท้ายจะได้ Homogeneous Transformation จาก Base ไปยัง Target หรือ ขวดที่เป็นเป้าหมายตามสมการข้างบน

Coding

สร้างฟังก์ชันสำหรับ Homogeneous Transformation โดย

tranx(a) = Translation ตามแกน X
trany(b) = Translation ตามแกน Y
tranz(c) = Translation ตามแกน Z
rotx(a) = Rotation ตามแกน X
roty(b) = Rotation ตามแกน Y
rotz(c) = Rotation ตามแกน Z

camtorobot(x,y) = ฟังก์ชันสำหรับเปลี่ยน X,Y ที่กล้องอ่านได้ เป็น Transformation Matrix สำหรับส่งค่าให้ Robot โดยวิธีการตามสมการที่กล่าวมาขั้นต้น

Trajectory Planning

เนื่องจาก Robot มีทั้ง 2 แขน แต่ Robot แขนขวานั้นมีการควบคุมแบบ Manual และ การจับตำแหน่งของขวดน้ำที่ไม่แน่นอน เพราะฉะนั้นจึงทำ Trajectory Planning เฉพาะแขนซ้าย ที่มีการควบคุมที่ Fix ตำแหน่งและเวลาเอาไว้แล้ว และ เมื่อพล๊อตกราฟออกมาจะได้

จะสังเกตุได้ว่า

Joint ที่ 1 นั้นมีกราฟที่ค่อนข้างมีความชัน เนื่องจาก แกนที่ 1 เป็นแกนที่ต้องเคลื่อนที่ยกขึ้นลง จากที่วางแก้วที่อยู่ชั้นที่ 1 ไปยังชั้นวางที่อยู่ชั้น 2 แล้วเคลื่อนที่กลับมา
Joint ที่ 6 นั้นมีการเปลี่ยนแปลงน้อยมากในแต่ละช่วงเวลา เพื่อที่จะควบคุมให้ Gripper และ แก้วน้ำนั้นตรงและหงายขึ้นตลอดเวลา