Inspecting Objects with Vision Camera [2 Hand Avatar Robot]

ผู้รับผิดชอบ — นายพิชามญชุ์ ทาดี [64340700420]

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

  • สามารถบอกพิกัดของวัตถุได้ [x,y]
  • สามารถส่งค่าพิกัดของวัตถุ [x,y] ไปยังหุ่นยนต์ได้

แผนภาพการทำงาน

  • ประกาศตัวแปรและฟังก์เริ่มต้นการใช้งาน ประกอบด้วย Homotransformation และ camtorobot
  • โปรแกรมจะทำการหากึ่งกลางของรูปภาพ และแปลงภาพ BGR2HSV เพื่อกำหนดช่วงสีเขียวใน HSV และสร้าง mask เพื่อตรวจจับสีเขียว
  • โปรแกรมจะทำการรอคำสั่งจากหุ่นยนต์ เพื่อทำการตรวจหาวงกลมของชิ้นงาน
  • โปรแกรมจะทำการวัดค่า [x,y] โดยเทียบจากจุดอ้างอิง (จุดติดตั้งกล้อง) เพื่อหาระยะ [x,y] ออกมา
  • โปรแกรมจะทำการเก็บค่า [x,y] ไว้ และส่งค่าไปคำนวณ camtorobot
  • โปรแกรมจะส่งค่า camtorobot ที่คำนวณเสร็จแล้ว ส่งไปยังหุ่นยนต์ในรูปแบบ BtoT (0x0000018EA896FA60 ค่าจะเปลี่ยนเมื่อตำแหน่งงานเปลี่ยน)
  • ค่า 0x0000018EA896FA60 (ค่าจะเปลี่ยนเมื่อตำแหน่งงานเปลี่ยน) เป็นที่อยู่หน่วยความจำ (memory address) ของออบเจกต์ใน Python ที่ถูกพิมพ์ออกมาเมื่อพยายามพิมพ์ออบเจกต์ฟังก์ชันโดยตรงแทนที่จะเรียกใช้งานฟังก์ชันนั้น

การทำงานของโปรแกรมและระบบประมวลผลการวิเคราะห์ภาพ

Original Image

HSV Image

Mask Image

Result Image

Project นี้ ใช้กล้อง EPL-USBFHD04H-SFV(2.8-12) ในการรับภาพ

  • cv2: ไลบรารี OpenCV ที่ใช้สำหรับการประมวลผลภาพและวิดีโอ
  • np: ไลบรารี NumPy ที่ใช้สำหรับการคำนวณทางคณิตศาสตร์และการจัดการกับอาร์เรย์
  • time: ไลบรารี time สำหรับการจัดการเวลาและการหน่วงเวลา
  • from foward import *: เป็นการนำเข้าโค้ดจากโมดูลชื่อ foward
  • pi = np.pi: กำหนดค่าของ pi เป็นค่าพายจากไลบรารี NumPy
  • loop = 0: กำหนดตัวแปร loop เริ่มต้นเป็น 0
  • cap = cv2.VideoCapture(0): เริ่มต้นการจับภาพจากกล้องเว็บแคม (device index 0)
  • timeout_start = time.time(): บันทึกเวลาปัจจุบันเมื่อเริ่มต้นการทำงาน
  • timeout_duration = 10: กำหนดระยะเวลา timeout เป็น 10 วินาที

โค้ดนี้เป็นการเตรียมพร้อมสำหรับการจับภาพจากกล้องและการตั้งเวลาเพื่อหยุดการทำงานหลังจากระยะเวลาที่กำหนดไว้ (timeout) เพื่อประมวลผลภาพและจัดการกับข้อมูลที่ได้รับจากกล้องเพิ่มเติม

  • tranx(a):สร้างเมทริกซ์การเลื่อนในทิศทางแกน X โดยระยะ a
  • trany(b):สร้างเมทริกซ์การเลื่อนในทิศทางแกน Y โดยระยะ b
  • tranz(c):สร้างเมทริกซ์การเลื่อนในทิศทางแกน Z โดยระยะ c
  • rotx(a):สร้างเมทริกซ์การหมุนรอบแกน X โดยมุม a
  • roty(b):สร้างเมทริกซ์การหมุนรอบแกน Y โดยมุม b
  • rotz(c):สร้างเมทริกซ์การหมุนรอบแกน Z โดยมุม c
  • camtorobot(center_distance_x_mm, center_distance_y_mm): ฟังก์ชันนี้ใช้สำหรับแปลงตำแหน่งจากพิกัดของกล้องไปยังพิกัดของหุ่นยนต์ โดยใช้เมทริกซ์การแปลงหลายตัวเพื่อคำนวณตำแหน่งสุดท้ายของวัตถุในระบบพิกัดของหุ่นยนต์
  • BtoF: เมทริกซ์การแปลงจากฐานหุ่นยนต์ไปยังเฟรมของภาพ
  • FtoT: เมทริกซ์การแปลงจากเฟรมของภาพไปยังตำแหน่งเป้าหมาย (ขวด) โดยมีการชดเชยระยะห่างของ Gripper
  • BtoT: เมทริกซ์การแปลงจากฐานหุ่นยนต์ไปยังตำแหน่งของขวด

โค้ดนี้เป็นชุดฟังก์ชันสำหรับการสร้างเมทริกซ์การแปลงแบบโฮโมจีนีส (homogeneous transformation matrices) ซึ่งใช้ในงานหุ่นยนต์และการแปลงพิกัดในสามมิติ (3D coordinate transformations) โดยเฉพาะในการคำนวณตำแหน่งและการหมุนของวัตถุ และโค้ดนี้ใช้ในการคำนวณตำแหน่งของวัตถุในระบบพิกัดของหุ่นยนต์จากข้อมูลพิกัดที่ได้จากกล้อง

  • เริ่มต้นลูปไม่รู้จบ (infinite loop)
  • ตรวจสอบเงื่อนไขของตัวแปร loop
  • ret: เป็นค่า boolean ที่ระบุว่าสามารถอ่านภาพจากกล้องได้สำเร็จหรือไม่
  • image: เป็นภาพที่อ่านได้จากกล้อง
  • image.shape[1]: ความกว้างของภาพ
  • image.shape[0]: ความสูงของภาพ
  • reference_point: พิกัดของจุดศูนย์กลางภาพ (แกน x และ y)
  • cv2.line วาดเส้นแนวนอนที่จุดศูนย์กลางของภาพ
  • cv2.cvtColor: ฟังก์ชันสำหรับแปลงภาพจากสี BGR เป็นสี HSV
  • กำหนดช่วงของสีเขียวใน HSV
  • cv2.inRange: ฟังก์ชันที่สร้างมาสก์เพื่อแยกส่วนของภาพที่อยู่ในช่วงสีที่กำหนด
  • เปลี่ยนค่าของตัวแปร loop และเริ่มลูปใหม่

โค้ดนี้เป็นส่วนหนึ่งของการประมวลผลภาพโดยใช้ไลบรารี OpenCV และ NumPy เพื่อจับภาพจากกล้อง, แปลงภาพเป็นสี HSV, และสร้างมาสก์เพื่อหาวัตถุที่มีสีเขียวในภาพ

  • cv2.waitKey(1): รอรับการกดปุ่มเป็นเวลา 1 มิลลิวินาที
  • if key == ord('s'): ตรวจสอบว่าปุ่ม ‘s’ ถูกกดหรือไม่
  • cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE): ค้นหาขอบเขตของวัตถุในภาพที่เป็นไปตามเงื่อนไขใน mask
  • วนลูปผ่านขอบเขตที่พบ และกรองขอบเขตที่มีพื้นที่น้อยกว่า 500 พิกเซล
  • cv2.minEnclosingCircle(contour): หาจุดศูนย์กลาง (x, y) และรัศมี r ของวงกลมที่ครอบขอบเขต
  • คำนวณระยะห่างจากจุดอ้างอิงในหน่วยพิกเซล
  • ใช้ค่าสัมประสิทธิ์ pixel_to_mm ที่กำหนดไว้ (50/82) เพื่อแปลงจากหน่วยพิกเซลเป็นมิลลิเมตร

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

  • การแปลงพิกเซลเป็นมิลลิเมตร
    • การแปลงจากพิกเซลเป็นหน่วยความยาวจริง (มิลลิเมตร) ต้องการค่าสัมประสิทธิ์ที่ถูกต้องซึ่งได้จากการวัดระยะทางจริงเทียบกับระยะทางในภาพ (พิกเซล)
    • สูตร: ระยะทางในมิลลิเมตร = ระยะทางในพิกเซล * ค่าสัมประสิทธิ์การแปลง (pixel_to_mm)
  • การคำนวณจุดศูนย์กลาง
    • จุดกึ่งกลางของภาพ: reference_point = (width // 2, height // 2)
  • การหาเส้นรอบวง
    • cv2.minEnclosingCircle: ใช้หาวงกลมที่เล็กที่สุดที่ครอบคลุมขอบเขตของวัตถุ
  • cv2.circle(image, center, r, (0, 255, 0), 4): วาดวงกลมสีเขียวที่ตำแหน่ง center และมีรัศมี r บนภาพ image โดยใช้เส้นที่มีความหนา 4 พิกเซล
  • cv2.putText(): วาดข้อความบนภาพ image ที่ตำแหน่งที่กำหนด (เช่น (20, 20)
  • cv2.imwrite(image_filename, image): บันทึกภาพ image ที่มีการวาดวงกลมและข้อความลงในไฟล์ Coordinated_X_Y_image.jpg
  • พิมพ์ข้อความยืนยันการบันทึกรูปภาพลงในไฟล์ Coordinated_X_Y_image.jpg

โค้ดนี้ทำหน้าที่วาดวงกลมที่ตรวจจับได้ลงบนภาพ, แสดงผลลัพธ์ที่เกี่ยวกับตำแหน่งและขนาดของวงกลม และบันทึกรูปภาพที่มีการวาดวงกลมและข้อมูลต่าง ๆ ลงในไฟล์

  • time.time() - timeout_start > timeout_duration: ตรวจสอบว่าระยะเวลาที่ผ่านมาเกิน timeout_duration (10 วินาที) หรือไม่
  • หาก timeout เกิดขึ้น จะทำการพิมพ์ค่าตัวแปร camtorobot และตั้งค่า loop กลับเป็น 0 เพื่อเริ่มต้นกระบวนการใหม่
  • cv2.imshow('Image', image): แสดงภาพต้นฉบับที่มีการวาดวงกลมและข้อมูลต่าง ๆ
  • saved_image = cv2.imread('Coordinated_X_Y_image.jpg'): อ่านไฟล์รูปภาพที่บันทึกไว้ (Coordinated_X_Y_image.jpg) และเก็บไว้ในตัวแปร saved_image
  • cv2.imshow('Coordinated_X_Y_image', saved_image): แสดงภาพที่บันทึกไว้
  • cv2.waitKey(0): รอการกดปุ่มใด ๆ ก่อนที่จะปิดหน้าต่าง
  • cv2.destroyAllWindows(): ปิดหน้าต่างทั้งหมดที่เปิดโดย OpenCV

โค้ดนี้มีหน้าที่ตรวจสอบว่ามีการหมดเวลา (timeout) หรือไม่ แสดงผลลัพธ์จากการคำนวณต่าง ๆ และควบคุมการแสดงผลของภาพ โดยที่

  • Timeout
    • หากเวลาที่ใช้ในลูปตรวจจับวงกลมเกินกว่า timeout_duration (10 วินาที) จะทำการพิมพ์ค่าตัวแปร camtorobot และตั้งค่า loop กลับเป็น 0 เพื่อเริ่มต้นกระบวนการตรวจจับใหม่
  • แสดงผลลัพธ์
    • แสดงภาพต้นฉบับ, ภาพ mask และภาพที่บันทึกไว้
  • การออกจากโปรแกรม
    • เมื่อผู้ใช้กดปุ่ม ‘q’ โปรแกรมจะออกจากลูปและปิดหน้าต่างแสดงภาพทั้งหมด

โปรแกรมคำนวณและสร้างเมทริกซ์การเปลี่ยนแปลงทางเรขาคณิต

โดยโปรแกรมจะมีการตั้งช่วงของสีเขียวไว้ และภาพที่นำเข้ามาในตอนแรกจะถูกแปลงเป็นปริภูมิสี HSV ก่อน

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

  • import numpy as np: นำเข้าไลบรารี NumPy เพื่อใช้งานฟังก์ชันและการคำนวณเมทริกซ์
  • def cal(a, b, d, q): กำหนดฟังก์ชัน cal ที่รับพารามิเตอร์ 4 ตัว คือ a, b, d, และ q
    • a: ระยะการเลื่อนในแกน x
    • b: มุม alpha, มุมการหมุนรอบแกน x
    • d: ระยะการเลื่อนในแกน z
    • q: มุม theta, มุมการหมุนรอบแกน z
  • การสร้างเมทริกซ์การเปลี่ยนแปลง x (Transformation Matrix)
  • การคืนค่าเมทริกซ์ (x)

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

  • การสร้างเมทริกซ์การเปลี่ยนแปลง (Transformation Matrix)
    • เมทริกซ์นี้มีขนาด 4×4 และประกอบด้วยส่วนที่เกี่ยวกับการหมุนและการเลื่อนในอวกาศ 3 มิติ
    • แถวแรกและแถวที่สองคำนวณจากฟังก์ชันตรีโกณมิติของ q (theta) และ b (alpha)
    • แถวที่สามประกอบด้วยการเลื่อนในแกน z โดยตรง
    • แถวที่สี่เป็นแถวของค่าคงที่ที่จำเป็นในเมทริกซ์การเปลี่ยนแปลง (homogeneous coordinates)
  • การใช้งานฟังก์ชัน cal
    • ฟังก์ชัน cal ใช้สำหรับการคำนวณเมทริกซ์การเปลี่ยนแปลงทางเรขาคณิตที่สามารถใช้ในการแปลงพิกัดของจุดจากเฟรมหนึ่งไปยังอีกเฟรมหนึ่ง
    • ตัวอย่างการใช้งาน
  • ฟังก์ชัน fowardkine รับพารามิเตอร์ joint ซึ่งเป็นลิสต์ของค่ามุมข้อต่อ (joint angles) สำหรับหุ่นยนต์แขนกล
  • ตั้งค่าการแสดงผลของ NumPy ให้มีความละเอียด 3 ตำแหน่งและแสดงผลเป็นรูปแบบ fixed-point
  • ค่า D-H Parameters แต่ละตัวประกอบด้วย
    • a: ระยะการเลื่อนในแกน x (link length)
    • alpha: มุมการหมุนรอบแกน x (link twist)
    • d: ระยะการเลื่อนในแกน z (link offset)
    • q: มุมการหมุนรอบแกน z (joint angle)
  • เรียกใช้ฟังก์ชัน cal เพื่อคำนวณเมทริกซ์การแปลงสำหรับแต่ละข้อต่อ โดยใช้ค่า D-H Parameters ที่กำหนดไว้
  • คำนวณเมทริกซ์การแปลงสุดท้ายโดยการคูณเมทริกซ์การแปลงของแต่ละข้อต่อเข้าด้วยกัน
  • หาตำแหน่งของจุดปลายของหุ่นยนต์ในอวกาศ 3 มิติจากเมทริกซ์การแปลงสุดท้าย

โค้ดนี้คำนวณการเคลื่อนที่ไปข้างหน้าของหุ่นยนต์แขนกล (forward kinematics) สำหรับหุ่นยนต์แขนกลที่มี 6 ข้อต่อ (6-DOF: degrees of freedom) โค้ดนี้ใช้ D-H (Denavit-Hartenberg) parameters เพื่อสร้างเมทริกซ์การแปลง (transformation matrices) สำหรับข้อต่อแต่ละข้อต่อและคำนวณตำแหน่งของจุดปลายของหุ่นยนต์ (tool point) ในอวกาศ 3 มิติ

  • การใช้งานฟังก์ชัน fowardkine
    • ฟังก์ชัน fowardkine ใช้เพื่อคำนวณตำแหน่งและการวางแนวของจุดปลายของหุ่นยนต์แขนกลจากค่ามุมข้อต่อ
    • ตัวอย่างการใช้งาน

สรุป

โปรแกรมนี้เป็นส่วนหนึ่งของการพัฒนาระบบที่ใช้กล้องเพื่อตรวจจับวัตถุและคำนวณตำแหน่งของวัตถุนั้นๆ โดยโปรแกรมมีลักษณะการทำงานดังนี้

  • เปิดการใช้งานกล้องเว็บแคมเพื่อเรียกใช้ภาพจากกล้อง
  • กำหนดตัวแปร loop เพื่อเป็นตัวควบคุมการทำงานของโปรแกรม โดยเริ่มต้นค่าที่ 0
  • กำหนดตัวแปร timeout_start เพื่อเก็บเวลาเริ่มต้นการรันโปรแกรม และ timeout_duration เพื่อกำหนดระยะเวลาสูงสุดที่โปรแกรมจะทำงาน
  • นิยามฟังก์ชันที่ใช้ในการแปลงตำแหน่งและมุมของวัตถุในภาพ เช่น tranx, trany, tranz, rotx, roty, rotz
  • เริ่มการวนลูป while ที่มีเงื่อนไขตรวจสอบค่าของ loop
  • ภายในลูป while ถ้า loop เท่ากับ 0 โปรแกรมจะอ่านภาพจากกล้อง เพื่อทำการวาดเส้นจุดอ้างอิงแกน x และ y บนภาพ และแปลงภาพให้เป็นรูปแบบ HSV เพื่อตรวจจับสีเขียว
  • โปรแกรมจะเข้าสู่สถานะ 1 เมื่อมีการกดปุ่ม ‘s’ จากนั้นโปรแกรมจะทำการค้นหาวงกลมในภาพ โดยการใช้เทคนิคการตรวจจับวงกลมจากภาพ HSV และหาศูนย์กลางและรัศมีของวงกลมที่พบ
  • หากโปรแกรมไม่พบวงกลมภายในเวลาที่กำหนด timeout_duration โปรแกรมจะแสดงข้อความ “Timeout: Circle not found.” และกำหนดค่า loop ให้เป็น 0 เพื่อเริ่มกระบวนการใหม่
  • แสดงผลลัพธ์บนหน้าต่าง Image และ mask โดยใช้ภาพที่ได้จากกล้อง และภาพประมวลผลที่เกิดจากการตรวจจับสีเขียว
  • นำรูปภาพที่ประมวลผลแล้วมาแสดงผลบนหน้าต่าง Coordinated_X_Y_image
  • หากผู้ใช้กดปุ่ม ‘q’ โปรแกรมจะหยุดการทำงาน และปิดหน้าต่างทั้งหมด

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

One comment

  1. Pingback: 2 Hand Avatar Robot – Human-Computer Interface Lab

Comments are closed.