Members
ธีรพจน์ แซ่ลิน
64340700408 (Poj)
กิตติพันธ์ เชษฐ์รัมย์
65340700401 (Mek)
ณัฐกร บัณฑิตขจร
65340700404 (Sun)
อาทิตย์ ศักดิ์สิทธิพิทักษ์
65340700407 (Pia)
Objectives
1. เพื่อศึกษาการประยุกต์ใช้ Forward and Inverse Kinematic (position) ในการควบคุมหุ่นยนต์ (Servo motor)
2. เพื่อศึกษาการประยุกต์ใช้ Velocity Analysis Jacobian and Trajectory Planning ในการควบคุมหุ่นยนต์ (Stepper motor)
3. เพื่อศึกษาการประยุกต์ใช้ Machine vision ในการตรวจจับสีและตำแหน่งวัตถุ
System Scenario
Step1: การหยิบรูบิค
Step2:
System Overview
ภาพรวมของโปรเจคจะใช้แขนสองตัวในการช่วย solve รูบิก โดยมีกล้องในการตรวจสอบหน้าทั้งหกโดยมีไฟปรับระดับแสงสว่างให้เหมาะสมกับการวิเคราะห์ภาพ
Design
Mechanical Design
Software Design
ใช้ Forward and Inverse Kinematic ในการแปลงค่า Position และ orientation ของปลาย tools มาเป็น joints variable โดยใช้ Matlab
Machine Vision Design
Progress_2
Mechanical Progress
สถานะโปรเจค ณ วันที่30/3/2023
1st Robot ที่ใช้ servo เป็น actuator ขึ้นโครงเรียบร้อย
เหลือ Implement ตัว gripper มาติดตั้ง
และสามารถหมุน joint ทั้ง 6 ได้แล้ว
Software Progress
ได้ทำการทำ Inverse Kinematics ในช่วงแขน (Arm) โดยใช้ Geometric solution แต่ยังมีปัญหาในการเปลี่ยนค่าจาก Task space เป็น Joint space
Test1
Step1: จำลองหุ่นและวางท่า เพื่อสร้าง Rotation Matrix (กรอบเขียว) และ Position X,Y,Z (กรอบเหลือง)
Step2: นำ Rotation Matrix (กรอบเขียว) ไปแปลงเป็น roll pitch yaw
Step3: แทนค่า roll pitch yaw (กรอบเขียว) และ Position X,Y,Z (กรอบเหลือง) ในโปรแกรมที่เขียนขึ้นมา จะได้ค่า joint parameter (กรอบแดง) ขึ้นมา
แต่จะเห็นว่าค่ายังไม่ตรงกันระหว่าง โปรแกรมจำลองและโปรแกรมที่ทางทีมงานเขียน แสดงให้เห็นว่ายังทำ inverse kinematic ยังไม่สำเร็จ
Test2
เมื่อทำการทดลองเหมือน test1 ข้างต้น แต่เปลี่ยนการวางตัวของหุ่น “ผลการทดลองก็ยังไม่สำเร็จเหมือนเดิม”
Progress_3
กำลังดีไซน์สามมิติในส่วนของหุ่นตัวที่สอง ความคืบหน้าช่วง Arm เสร็จละ เหลือช่วง Wrist
Progress_4
กำลังอยู่ในขั้นตอนการขึ้นโครงสร้างโดยใช้เครื่องพิมพ์สามมิติ(พิมพ์ส่วนที่คอนเฟิร์มแล้วว่าจะไม่มีการแก้ไขแล้ว)
โดยหุ่นตัวที่สองนี้จะใช้ Stepper motor แทนที่จะเป็น Servo motor แบบในหุ่นตัวที่หนึ่ง
Progress_5
Flow_Chart แสดงการเช็ค position และ orientation ของ Rubik
และตรวจสอบว่ามี Rubik ใน working space ที่กำหนดหรือไม่
Flow_Chart แสดงการทำงานของ Rubik Solving Process
Gripper ที่มีการออกแบบใหม่
Final Present
1.System Scenario
โจทย์ใน class project นี้คือให้สร้างหุ่น serial robot ในการ solve rubik ซึ่งจะใช้หุ่นทั้งหมด 2 ตัวช่วยกัน pick, place, หมุนรูบิค โดยจะมีการใช้กล้องในการระบุ position และ orientation ของรูบิคว่าอยู่ตรงไหนแล้วจะให้เข้าไปกระทำกับรูบิคยังไงบ้าง
แต่เนื่องด้วยเวลาที่จำกัดส่งผลให้เราลดขอบเขตงานจากการใช้หุ่น 2 ตัวเหลือตัวเดียว แล้วใช้ jig เป็นตัวช่วยในการบิดรูบิคเพื่อเปลี่ยนหน้าแทน
โดยเป้าหมายยังคงเดิมคือ นำ Kinematic และ Trajectory planing มาประยุกต์ในการ solve rubik โดยใช้ robot vision ในการระบุ position และ orientation ซึ่งอยู่ใน task space แล้วทำ Inverse kinemetic เพื่อแปลงเป็น joint space ที่จะนำไปควบคุมการเคลื่อนที่ของหุ่นอีกที
2. Over all communicate System
ภาพรวม : เริ่มจาก Python ตรวจสอบตำแหน่งแล้วทำการบันทึกลงไฟล์ text จากนั้นทาง matlab จะทำการตรวจสอบค่าที่เปลี่ยนไปในไฟล์ text และนำมาคำนวน IK เมื่อได้ Parameter ของแต่ละ joint มาแล้วก็จะทำการส่งต่อไปยัง Python ผ่านการสื่อสาร Socket Server จากนั้น Python จะส่งค่าต่อไปยัง Arduino เพื่อทำการควบคุมแขนกลต่อไป
3.Hardware design
ในด้าน mechanic design หุ่นยนต์เราที่จะนำมาแก้ไขปัญหา Rubik’s cube จะเป็นหุ่นยนต์ 6 DOF ดังนั้นเมื่อเราได้รูปแบบของการวาง Frame ของแต่ละ Join และ Concept ของหุ่นยนต์เบื่องต้นแล้วจากนั้นเราจึงมาออกแบบและเลือก Actuator ของหุ่นยนต์ เพื่อทำการออกแบบให้สามารถใช้งานได้จริง โดยเป้าหมายของเราจะเป็น การยก Rubik’s cube เราจึงจำเป็นต้อง หาน้ำหนังของ Rubik’s cube เพื่อให้ได้ปริมาญของจุดปลายที่จะรับไว้สูงสุด ที่จุดปลาย ด้วยข้อมูลนี้กลุ่มเราจึงเลือกใช้ Servo motor ที่ 150/cm เพื่อรับ โหลดน้ำหนักที่จุดปลาย และนำมาออกแบบต่อ และในส่วนฐานเราก็ได้เพื่มเติม Bearing เข้ามารับแรงในแนวแกนต่าง ๆ อีกด้วย
4.FK&IK Finding
ทำ Forward Kinematic โดยการใช้ dh convention จากการวางตัวของ Joint ต่างๆ ในหุ่นของเรา (Step1 –> 3)
และเนื่องด้วยหุ่นของทางทีมงานใช้เป็น Serial robot 6 dof ที่สามารถทำ Inverse kinematic เพื่อหา joint parameter ได้ แต่ทำได้ยาก
ดังนั้นจึงทำกระบวนการ decoupling เพื่อแยกช่วง Arm และ Wrist โดยจะได้จตำแหน่งข้อมือออกมา ซึ่งก็คือ Xc Yc Zc (Step4 –> 5)
เมื่อได้ช่วง Arm ก็ทำ Inverse kinematic โดยทางทีมงานเลือกใช้เป็น Geometric Approach เพื่อหา Theta 1 ถึง 3 (Step6)
และเมื่อได้ Theta 1 ถึง 3 ของช่วง Arm ก็ทำ Inverse kinematic ของช่วง wrist โดยทางทีมงานเลือกใช้เป็น Algebraic Approach เพื่อหา Theta 4 ถึง 6 (Step7 –> 9)
5. Software Design
Get position and orientation from the image processing process
Flow Chart 1: start ให้วางรูบิคใน working space (อาจอยู่นอก working space) แล้วกดปุ่ม enter เพื่อรับการจับภาพโดยภาพจะมีการหมุนเพื่อรองรับการทำ orientation ด้วย โดยการหมุนภาพตามเข็มสลับทวนเข็ม ถ้ามีรูบิคใน working space จะทำการแคปภาพเพื่อส่งข้อมูล x,y coordinate และ orientation
ภาพแสดง position y,x ที่ได้จากตัวกล้อง
Get the 6 faces of Rubik to find the array of solution
Flow Chart 2 : Process A —-> เมื่อคำนวณ inverse kinematic จาก matlab เพื่อบอกตำแหน่งและการวางตัวในการเข้าไปหยิบ จากนะั้นแขนกลจะทำการเข้าทำในท่า “pickUp_face” > “Move to_initialPosition” > putDown_face (ที่ตำแหน่งหน้ากล้อง) เพื่อทำการตรวจสอบรูบิคในแต่ละหน้า ทำวนเป็น loop จนตรวจสอบครบทั้ง 6 หน้า จากนั้นแขนกลจะกลับสู่ตำแหน่ง Iinitial —> Process B
ภาพแสดงเมื่อหุ่นยนต์หมุนหน้ารูบิคให้กล้องตรวจจับครบหกหน้า
Process to solve the Rubik
ภาพแสดงเหตุการณ์เมื่อส่งหน้าทั้งหกของรูบิกเข้ากระบวนการหา solving solution จะได้ Command Array
Flow Chart 3 :เมื่อเราได้ข้อมูลของ Rubik’s cube ทั้ง 6 ด้านจากการ Capture แล้วระบบจะทำการ generate วิธีการแก้ปัญหา Rubik’s cube ออกมา จากนั้นข้อมูลจะถูกส่งมาขั้นตอนถัดมาและเช็คเงื่อนไข ต่างๆ หากข้อมูลเป็นอักษรปกติ F B L R U P จะหมุนตามเข็ม นาฬิกา แต่ถ้าหากข้อมูลมี F’ B’ L’ R’ U’ P’ จะหมุนทวนเข็ม จากนั้น แปรแกรมก็จะวน Loop ไปเช็คเงื่อนไข จนกว่า Rubik’s cube จะครบเงื่อนไข และเมื่อครบเงื่อนไขแล้วโปรแกรมจะจบการทำงาน
หลักการหาเรขาคณิตในรูปแบบ 3×3 (หน้ารูบิค)
เริ่มจากบันทึกภาพจากกล้องเว็บแคมแล้วทำการเป็น gray frame ก่อนแล้วเบลอเฟรมด้วย filter ขนาด 3×3 แล้วใส่ canny frame ให้เป็นขอบรูปออกมา แล้วใส่ kernel ทำ dilated frame แล้วทำเรื่อง find coutours เป็นเรื่องการหารูปร่างที่มีวงปิด แล้วหารูปร่างที่มีรูปร่างแปดทิศล้อมรอบแล้วดึงค่ารูปร่างเป็น array แล้วนับว่ามีทั้งหมด 9 รูปร่างไหม แล้วทำการวาดรูปแล้วดึงค่า pixel x y ออกมา
การเขียนดักเพื่อไม่ให้เกิด Worst Case ในการทำงานของหุ่นยนต์
หลักๆจะมี 2 ส่วนที่เขียนขึ้นมาเพื่อป้องกัน
- การเขียนดักในกรณีที่มีการตรวจสอบ position ของรูบิคจากกล้อง และ position ที่ได้จากการนำ Joint Parameter ที่หาได้จาก IK นำไปคำนวนใน FK จะต้องได้ position ที่ “เท่ากัน”
ตัวอย่าง code สำหรับดักเคสที่ 1
2. ค่า Joint Parameter ที่ได้จากการคำนวน IK จะต้องมีค่าที่เปลี่ยนไปไม่เกิน 135 องศา ทั้งด้านบวกและลบ เนื่องจากค่า Initial ของแขนกลคือ 135 องศาและ servo motor ที่นำมาใช้หมุนได้ 0 -270 องศา
ตัวอย่าง code สำหรับดักเคสที่ 2
การหา Homogeneous tranformatiom ระหว่างเฟรมกล้องเทียบเบสเฟรมของหุ่น
หา Homogeneous tranformatiom ระหว่างเฟรมกล้องเทียบเบสเฟรมของหุ่น
เพื่อที่จะได้เปลี่ยนเฟรมอ้างอิงของรูบิคจากเฟรมกล้องไปเป็นเบสเฟรม แล้วนำจุดที่อ้างอิงกับเบสเฟรมไปคำนวณ Inverse Kinematics
Jacobian Matrix
หุ่นหมายเลขที่มีความต้องการประยุกต์ใช้ jacobian matrix ในการแปลงความเร็วใน task space เป็น ความเร็วใน joint space
Trajectories planing
Concept ในการทำ Trajectories โดยใช้ stepper motor ของกลุ่มเราคือ
ภาพที่1: เราจะใช้ joint space trajectories โดยการนำจุดต้นจุดปลายมาทำ Invert kinematics จนรู้ว่าแต่ละ joint ต้องหมุนเพิ่มไปกี่องศา
ภาพที่2: หลังจากนั้นมากำหนดว่าในหนึ่งช่วงเวลาจะย่อยเป็นกี่ช่วง ซึ่งจะสังเกตุว่าช่วงของแต่ละ joint จะใช้องศาต่อช่วงที่แตกต่างกัน
joint ไหนที่เริ่มและจบที่มีองศาที่กว้าง ช่วงย่อยก็จะใช้องศาในการป้อนเข้า servo มาก ในทางกลับกัน joint ไหนที่เริ่มและจบที่มีองศาที่น้อย ช่วงย่อยก็จะใช้องศาในการป้อนเข้า servo น้อย ทางทีมงานจะนำ Concept นี้ไปใช้ในการควบคุม joint ทั้งหกของหุ่นยนต์
Testing Video
Thank you for your attention