หุ่นยนต์และระบบอัตโนมัติ

ทำไมต้องรู้

UAT 314 และ UAT 202 วางภาพรวมของระบบอัตโนมัติ PLC แขนกลสองข้อต่อ และหุ่นยนต์สองล้อ วิชานี้เป็นวิชาปฏิบัติที่ลงมือสร้างระบบหุ่นยนต์ให้ทำงานร่วมกับโดรน กรณีที่ใช้ตลอดวิชาคือ โซลาร์ฟาร์ม ที่มีรถหุ่นยนต์ภาคพื้น (UGV) วิ่งตามแถวแผง ใช้กล้องความร้อนหาจุดร้อนบนแผง และพาโดรนไปปล่อยสำรวจมุมสูง หน่วยความรู้เรื่อง Robotics & Autonomous Systems ของคลังความรู้โดรนกล่าวถึงระบบปฏิบัติการหุ่นยนต์และการประสานหลายระบบ ส่วนหน่วยความรู้เรื่องระบบอัตโนมัติอุตสาหกรรมและ PLC อธิบายแอคชูเอเตอร์และลอจิกควบคุม โมดูลแรกเริ่มจากเรื่องที่หุ่นยนต์ทุกตัวต้องทำให้ถูกก่อนเรื่องอื่น คือรู้ว่า อะไรอยู่ที่ไหน ในกรอบพิกัดใด

องค์ประกอบของหุ่นยนต์เคลื่อนที่

ตำราของ Siegwart และคณะแบ่งงานของหุ่นยนต์เคลื่อนที่เป็น การรับรู้ (เซนเซอร์) การระบุตำแหน่ง การวางแผน และการควบคุมการเคลื่อนที่ (แอคชูเอเตอร์) รถ UGV ในโซลาร์ฟาร์มมี encoder ที่ล้อ IMU GNSS กล้องความร้อน และ LiDAR ขับด้วยมอเตอร์ไฟฟ้ากระแสตรงสองตัว ตัวควบคุมคือคอมพิวเตอร์บนรถที่รัน ROS 2 และสื่อสารกับศูนย์ควบคุมผ่านเครือข่ายไร้สาย ข้อมูลจากเซนเซอร์แต่ละตัวอยู่ใน กรอบพิกัดของตัวเอง จึงต้องแปลงให้อยู่ในกรอบเดียวกันก่อนนำมาใช้ร่วมกัน

การแปลงแบบเอกพันธ์และต้นไม้กรอบพิกัด

ตำแหน่งและทิศของกรอบหนึ่งเทียบกับอีกกรอบเขียนเป็นเมทริกซ์ 3×3 ใน 2 มิติ (4×4 ใน 3 มิติ) ที่รวมการหมุนและการเลื่อนไว้ด้วยกัน การต่อกรอบทำได้ด้วยการคูณเมทริกซ์ตามลำดับจากกรอบใหญ่ไปกรอบเล็ก REP 103 กำหนดหน่วย SI และแกนของตัวหุ่นยนต์ให้ x ชี้ไปข้างหน้า y ชี้ไปทางซ้าย z ชี้ขึ้น ไลบรารี tf ของ ROS (Foote, 2013) เก็บความสัมพันธ์ของกรอบเป็นต้นไม้และคำนวณการแปลงให้อัตโนมัติ

import numpy as np

def se2(x, y, deg):
    t = np.radians(deg)
    return np.array([[np.cos(t), -np.sin(t), x],
                     [np.sin(t),  np.cos(t), y],
                     [0, 0, 1]])

T_map_base = se2(12.0, 5.0, 30)       # หุ่นยนต์บนแผนที่ หันหัว 30°
T_base_cam = se2(0.20, -0.25, -90)    # กล้องติดด้านขวา หันออกขวา
p_cam = np.array([4.0, 0.5, 1])       # จุดร้อนบนแผงที่กล้องเห็น

p_map = T_map_base @ T_base_cam @ p_cam
print(f"hotspot on map: ({p_map[0]:.2f}, {p_map[1]:.2f}) m")

wrong = T_base_cam @ T_map_base @ p_cam
print(f"wrong multiplication order: ({wrong[0]:.2f}, {wrong[1]:.2f}) m")

goal = np.linalg.inv(T_map_base) @ np.array([20.0, 9.0, 1])
print(f"goal in robot frame: x = {goal[0]:.2f} m, y = {goal[1]:.2f} m")

hotspot on map: (14.73, 1.67) m
wrong multiplication order: (7.63, -15.46) m
goal in robot frame: x = 8.93 m, y = -0.54 m

map, odom และ base_link

REP 105 กำหนดกรอบหลักของหุ่นยนต์เคลื่อนที่ไว้สามกรอบ base_link ติดกับตัวรถ odom เป็นกรอบที่ตำแหน่งเปลี่ยนต่อเนื่องไม่กระโดด แต่ลอยสะสมตามระยะทาง เหมาะกับการควบคุมระยะสั้น ส่วน map เป็นกรอบที่ตำแหน่งไม่ลอยในระยะยาว แต่อาจกระโดดได้เมื่อระบบระบุตำแหน่งปรับแก้ เช่นเมื่อได้ GNSS หรือจับคู่กับแผนที่ได้ แยกกรอบแบบนี้ทำให้ตัวควบคุมล้อไม่สะดุดเมื่อตำแหน่งบนแผนที่ถูกแก้

odometry และความคลาดเคลื่อนเชิงระบบ

odometry คือการประมาณตำแหน่งจากจำนวนรอบล้อ หุ่นยนต์ขับเคลื่อนสองล้อหันเมื่อสองล้อวิ่งได้ระยะต่างกัน $\Delta \theta =(d_R-d_L)/b$ ถ้ารัศมีล้อที่ซอฟต์แวร์ใช้ไม่ตรงกับของจริง ความผิดพลาดจะสะสมไปทางเดียวเสมอ Borenstein และ Feng (1996) ระบุสาเหตุหลักสองอย่างคือ เส้นผ่านศูนย์กลางล้อไม่เท่ากัน และระยะห่างล้อที่ไม่แน่นอน และเสนอวิธีทดสอบ UMBmark ที่ให้หุ่นยนต์เดินเส้นทางสี่เหลี่ยมจัตุรัสทั้งตามและทวนเข็มนาฬิกาเพื่อแยกสองสาเหตุนี้ออกจากกัน

import numpy as np

b = 0.50            # ระยะห่างล้อ m
r_nom = 0.100       # รัศมีล้อที่ซอฟต์แวร์ใช้ m
r_right = 0.1001    # ล้อขวาจริงใหญ่กว่า 0.1%
dist = 50.0         # ระยะที่ซอฟต์แวร์คิดว่าวิ่งตรง m

rev = dist / (2 * np.pi * r_nom)          # จำนวนรอบเท่ากันทั้งสองล้อ
d_left = rev * 2 * np.pi * r_nom
d_right = rev * 2 * np.pi * r_right
dtheta = (d_right - d_left) / b
R = b * (d_right + d_left) / (2 * (d_right - d_left))
lateral = R * (1 - np.cos(dtheta))
print(f"actual heading change {np.degrees(dtheta):.2f} deg")
print(f"actual turn radius {R:.0f} m")
print(f"sideways offset at the end {lateral:.2f} m")

actual heading change 5.73 deg
actual turn radius 500 m
sideways offset at the end 2.50 m

ปฏิบัติการประจำโมดูล

ข้อผิดพลาดที่พบบ่อย

สรุป

• หุ่นยนต์เคลื่อนที่ประกอบด้วยการรับรู้ การระบุตำแหน่ง การวางแผน และการควบคุม
• การแปลงแบบเอกพันธ์รวมการหมุนกับการเลื่อน และต่อกรอบได้ด้วยการคูณตามลำดับ
• REP 105 แยก map, odom และ base_link ตามคุณสมบัติความต่อเนื่องและการลอย
• odometry สะสมความคลาดเคลื่อนเชิงระบบ ต้องสอบเทียบและใช้แหล่งอื่นช่วยแก้

แบบฝึกตรวจความเข้าใจ

1. ตามแนวปฏิบัติ REP 103 แกน y ของตัวหุ่นยนต์ชี้ไปทางใด
2. กรอบใดตำแหน่งต่อเนื่องแต่ลอยสะสม และกรอบใดไม่ลอยแต่อาจกระโดด
3. ล้อขวาวิ่งได้ 10.02 m ล้อซ้าย 10.00 m ระยะห่างล้อ 0.5 m หุ่นยนต์หันไปกี่เรเดียน
4. ถ้าเป้าหมายในกรอบรถมีค่า y เป็นลบ เป้าหมายอยู่ทางใดของรถ
5. การทดสอบ UMBmark ให้หุ่นยนต์เดินทั้งตามและทวนเข็มนาฬิกาเพื่ออะไร