ผู้รับผิดชอบ — นายพิชามญชุ์ ทาดี [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
เป็นค่าพายจากไลบรารี NumPyloop = 0
: กำหนดตัวแปรloop
เริ่มต้นเป็น 0cap = 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
: เมทริกซ์การแปลงจากเฟรมของภาพไปยังตำแหน่งเป้าหมาย (ขวด) โดยมีการชดเชยระยะห่างของ GripperBtoT
: เมทริกซ์การแปลงจากฐานหุ่นยนต์ไปยังตำแหน่งของขวด
โค้ดนี้เป็นชุดฟังก์ชันสำหรับการสร้างเมทริกซ์การแปลงแบบโฮโมจีนีส (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
: ระยะการเลื่อนในแกน xb
: มุมalpha
, มุมการหมุนรอบแกน xd
: ระยะการเลื่อนในแกน zq
: มุม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’ โปรแกรมจะหยุดการทำงาน และปิดหน้าต่างทั้งหมด
โค้ดนี้ถูกออกแบบเพื่อทำงานร่วมกับการทำนายตำแหน่งของวัตถุโดยใช้โมเดลการตรวจจับวัตถุ และการคำนวณตำแหน่งของวัตถุนั้นๆ บนโปรแกรมอื่นๆ อาจใช้เพื่อควบคุมหุ่นยนต์หรือการทำงานที่เกี่ยวข้องกับการตรวจจับวัตถุในโลกจริง
Pingback: 2 Hand Avatar Robot – Human-Computer Interface Lab