รูปปกบทความ

1. 🎯 ตอนที่ 24: Odometry หุ่นยนต์รู้ได้อย่างไรว่าตัวเองอยู่ที่ไหน?

สวัสดีครับน้องๆ วิศวกรหุ่นยนต์ทุกคน! ในตอนก่อนๆ เราได้เรียนรู้วิธีการส่งคำสั่งความเร็ว (Twist) ให้ล้อหุ่นยนต์หมุนตามสมการ Kinematics กันไปแล้ว

แต่การที่หุ่นยนต์จะเดินไปถึงเป้าหมายได้ มันต้องตอบคำถามปรัชญาสุดคลาสสิกให้ได้ก่อนครับว่า… “ตอนนี้ฉันอยู่ที่ไหน?” ถ้าหุ่นยนต์ไม่รู้ตำแหน่งปัจจุบันของตัวเอง ระบบนำทาง (Navigation) ก็ไม่สามารถคำนวณเส้นทางต่อไปได้เลย! วันนี้พี่จะพาไปเจาะลึกศาสตร์แห่งการนับก้าวเดินที่เรียกว่า Odometry และกระชากหน้ากากผู้ร้ายที่ทำให้หุ่นยนต์ของเราเดินหลงทิศ (Drift) เตรียมตัวให้พร้อมครับ!

2. 📖 เปิดฉาก (The Hook)

สมัยที่พี่ทำหุ่นยนต์ตัวแรก พี่เขียนโค้ดสั่งให้มัน “เดินหน้าตรงไป 10 เมตร” พอรันปุ๊บ หุ่นยนต์ก็เดินหน้าไปอย่างมั่นใจครับ… แต่มันดันไปหยุดอยู่ที่ระยะ 9.9 เมตร แถมยังเป๋ออกไปทางซ้ายอีกเกือบ 1 เมตร! ทั้งๆ ที่โค้ดก็เขียนถูก ล้อก็หมุนด้วยความเร็วเท่ากันเป๊ะ ทำไมมันถึงไม่ตรงล่ะ?

ในโลกความเป็นจริง หุ่นยนต์ของเราเปรียบเสมือนคนที่ “ถูกปิดตา” ครับ การที่มันจะรู้ว่าตัวเองเดินมาไกลแค่ไหน มันทำได้แค่การ “นับก้าว” ของตัวเอง (Dead Reckoning) ปัญหาคือล้อหุ่นยนต์อาจจะลื่น (Slip) ยางอาจจะแบนไม่เท่ากัน หรือพื้นอาจจะขรุขระ ทำให้ก้าวที่มันนับได้ ไม่เท่ากับระยะทางที่มันเดินไปจริงๆ! กระบวนการเอาข้อมูลจากเซ็นเซอร์ภายในร่างกายตัวเองมาคำนวณหาตำแหน่งนี้ เราเรียกว่าการทำ Odometry ครับ ซึ่งเป็นพื้นฐานที่สำคัญที่สุดก่อนที่เราจะไปทำแผนที่ขั้นสูงกัน

3. 🧠 แก่นวิชา (Core Concepts)

ระบบ Odometry จะถูกคำนวณและส่งออกไปใน ROS 2 ผ่าน Topic มาตรฐานที่ชื่อว่า /odom (ชนิดข้อความ nav_msgs/Odometry) โดยอาศัยข้อมูลจาก 2 เซ็นเซอร์หลัก:

  • Wheel Encoders (การนับก้าวจากล้อ): ที่มอเตอร์ของหุ่นยนต์จะมีเซ็นเซอร์ที่เรียกว่า Encoder คอยนับ “Tick” หรือจำนวนพัลส์เวลาที่เพลามอเตอร์หมุน เมื่อเรารู้ว่าล้อมีรัศมี ($R$) เท่าไหร่ และ 1 รอบมีกี่ Tick เราก็จะคำนวณระยะทางที่ล้อแต่ละข้างกลิ้งไปได้ ($\Delta D_L, \Delta D_R$) จากนั้นเราจะนำมาเข้าสมการ Forward Kinematics เพื่อหาระยะที่หุ่นยนต์เคลื่อนที่ไปข้างหน้า ($\Delta s$) และมุมที่หุ่นยนต์หมุนเปลี่ยนไป ($\Delta \theta$)
  • ความคลาดเคลื่อนสะสม (Odometry Drift): การพึ่งพาแค่ Encoder มีจุดอ่อนร้ายแรงครับ เพราะมันคือการเอาตำแหน่งเก่ามาบวกกับก้าวใหม่ (Integration) ถ้าล้อเกิดการฟรีทิ้ง (Skidding) หุ่นยนต์จะคิดว่ามันเดินไปแล้ว แต่จริงๆ ยังอยู่กับที่! หรือถ้าล้อสองข้างเติมลมมาไม่เท่ากัน รัศมีล้อเพี้ยนไปแค่ 5 มิลลิเมตร เมื่อวิ่งไปสักพัก ค่า Error จะสะสม (Accumulate) จนหุ่นยนต์คิดว่าตัวเองอยู่คนละซอยเลยครับ!
  • Inertial Measurement Unit - IMU (หูชั้นในของหุ่นยนต์): เพื่อแก้ปัญหาล้อฟรี วิศวกรจึงต้องติดเซ็นเซอร์ IMU (ประกอบด้วย Accelerometer และ Gyroscope) เข้าไปช่วย IMU จะวัดความเร่งและอัตราการหมุน (Angular Velocity) ได้โดยตรงแบบไม่ต้องง้อล้อ โดยเฉพาะข้อมูลมุมหัน (Yaw/Heading) จาก Gyroscope นั้นมีประโยชน์มหาศาลในการช่วยลด Error การเลี้ยวของหุ่นยนต์ครับ
รูปประกอบ Odometry Calculation and Drift

4. 💻 ร่ายมนต์โค้ดและคำสั่ง (Show me the Code)

เรามาดูหัวใจของการคำนวณ Odometry จาก Wheel Encoders ด้วย Python กันครับ โค้ดนี้จะรับค่า Tick จากล้อซ้าย-ขวา มาอัปเดตพิกัด (X, Y, Theta) ของหุ่นยนต์:

โครงสร้างคลาสคำนวณ Odometry (wheel_odometry.py):

import math
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion

class WheelOdometry(Node):
    def __init__(self):
        super().__init__('wheel_odometry_node')
        
        # 1. กำหนดพารามิเตอร์ทางกายภาพของหุ่นยนต์
        self.wheel_radius = 0.05  # รัศมีล้อ (เมตร)
        self.wheel_separation = 0.3  # ระยะห่างระหว่างล้อซ้าย-ขวา (เมตร)
        self.ticks_per_rev = 2000  # จำนวน Tick ต่อการหมุน 1 รอบ
        
        # 2. ตัวแปรเก็บตำแหน่งปัจจุบันของหุ่นยนต์ (X, Y, Theta)
        self.x = 0.0
        self.y = 0.0
        self.theta = 0.0
        
        # 3. ตัวแปรเก็บค่า Tick เก่า
        self.prev_left_ticks = 0
        self.prev_right_ticks = 0

    # 4. ฟังก์ชันอัปเดต Odometry (เรียกใช้ทุกครั้งที่อ่านค่า Encoder ได้)
    def update_odometry(self, left_ticks, right_ticks):
        # คำนวณ Tick ที่เปลี่ยนไป
        delta_left = left_ticks - self.prev_left_ticks
        delta_right = right_ticks - self.prev_right_ticks
        
        # แปลง Tick เป็นระยะทาง (เมตร)
        distance_per_tick = (2 * math.pi * self.wheel_radius) / self.ticks_per_rev
        d_left = delta_left * distance_per_tick
        d_right = delta_right * distance_per_tick
        
        # คำนวณระยะทางรวมและมุมที่เปลี่ยนไป (Differential Drive Kinematics)
        d_center = (d_left + d_right) / 2.0
        delta_theta = (d_right - d_left) / self.wheel_separation
        
        # อัปเดตพิกัด X, Y (ใช้ Trigonometry แตกเวกเตอร์)
        self.x += d_center * math.cos(self.theta + (delta_theta / 2.0))
        self.y += d_center * math.sin(self.theta + (delta_theta / 2.0))
        self.theta += delta_theta
        
        # เซฟ Tick ไว้ใช้รอบหน้า
        self.prev_left_ticks = left_ticks
        self.prev_right_ticks = right_ticks
        
        return self.x, self.y, self.theta

# คำสั่ง Terminal สำหรับดูค่า Odometry ที่ถูก Publish ออกมา
# ros2 topic echo /odom

คอมเมนต์สไตล์รุ่นพี่: สังเกตบรรทัดอัปเดต self.x และ self.y ไหมครับ? เราบวก (delta_theta / 2.0) เข้าไปในมุมด้วย เพราะในความเป็นจริงหุ่นยนต์เคลื่อนที่เป็น “เส้นโค้ง (Arc)” ไม่ใช่เส้นตรง การทำแบบนี้ (Runge-Kutta integration) จะให้ความแม่นยำสูงกว่าการใช้มุม self.theta เดิมตรงๆ ครับ!

5. 🛡️ เคล็ดลับจากคัมภีร์ลับ (Under the Hood / Pro-Tips)

ในฐานะที่พี่สู้รบกับ Odometry Drift มาอย่างโชกโชน นี่คือข้อควรระวังขั้นสุดครับ:

  • Error มุม (Heading Error) คือหายนะ: สมมติว่าหุ่นยนต์คำนวณมุม (Theta) พลาดไปแค่ 0.1 เรเดียน (ประมาณ 5.7 องศา) ถ้าน้องสั่งให้มันวิ่งตรงไป 10 เมตร Error ในแกน X, Y จะบานปลายไปถึงเกือบ 1 เมตรเลยครับ! ดังนั้น การนำค่าจาก Gyroscope (IMU) ที่แม่นยำกว่ามา “ฟิวชัน (Fusion)” แทนที่การคำนวณมุมจากล้อ จึงเป็นเรื่องที่ “ต้องทำ” เสมอครับ!
  • คำสาปของ IMU Bias: ถึง IMU จะช่วยเรื่องมุมได้ดี แต่มันก็มีปัญหาของมันครับ นั่นคือ “Bias Offset” หรือค่าสัญญาณรบกวนที่ติดมาแม้เซ็นเซอร์จะวางอยู่นิ่งๆ ถ้าน้องเอาค่าความเร่ง (Acceleration) จาก IMU มาอินทิเกรตหาตำแหน่งตรงๆ โดยไม่ผ่านฟิลเตอร์ หุ่นยนต์ของน้องจะคิดว่าตัวเองกำลังลอยออกไปนอกโลกทั้งๆ ที่จอดอยู่เฉยๆ ครับ!
  • ใช้ EKF เข้ามาช่วย (Sensor Fusion): ใน ROS 2 เราจะไม่ปล่อยให้ Encoder หรือ IMU ทำงานเดี่ยวๆ ครับ แต่เราจะใช้แพ็กเกจอย่าง robot_localization ซึ่งเป็น Extended Kalman Filter (EKF) มาทำหน้าที่จับข้อมูลทั้งสองแหล่งมายำรวมกัน (Fuse) EKF จะรู้ว่าตอนไหนควรเชื่อล้อ (เช่น ตอนหุ่นยนต์วิ่งทางตรง) และตอนไหนควรเชื่อ IMU (เช่น ตอนหุ่นยนต์หมุนตัวหรือล้อฟรี) ทำให้เราได้ค่า /odom ที่เนียนกริบ!

6. 🏁 บทสรุป (To be continued…)

Odometry คือการที่หุ่นยนต์รับรู้การเคลื่อนที่ของตัวเองผ่านเซ็นเซอร์ภายใน (Proprioceptive sensors) อย่าง Wheel Encoders และ IMU ข้อมูลนี้มีความถี่สูงและตอบสนองไวมาก เหมาะสำหรับเอาไปใช้ทำ Feedback Control คุมมอเตอร์ครับ

แต่ข้อจำกัดคือ “มันไว้ใจระยะยาวไม่ได้ (Drift)”! เมื่อหุ่นยนต์วิ่งไปเรื่อยๆ โลกในหัวของมันกับโลกความเป็นจริงจะเริ่มบิดเบี้ยวออกจากกัน เพื่อที่จะรีเซ็ตค่า Error เหล่านี้ให้กลับมาเป็นศูนย์ หุ่นยนต์จะต้องหันไปพึ่งพาเซ็นเซอร์ภายนอก (Exteroceptive sensors) เช่น LIDAR หรือกล้อง เพื่อมองหาจุดอ้างอิงในห้อง

ในตอนต่อไป พี่จะพาน้องๆ ไปพบกับเทคนิคระดับเทพที่ชื่อว่า SLAM (Simultaneous Localization and Mapping) ที่จะเอาข้อมูล LIDAR มาต่อจิ๊กซอว์สร้างแผนที่ พร้อมกับจับผิด Odometry ไปในตัว เตรียมตัวตื่นเต้นกันได้เลยครับ!

ต้องการที่ปรึกษาด้านการออกแบบสถาปัตยกรรมหุ่นยนต์ (Robotics) และระบบ Automation ให้กับองค์กรของคุณ? ทีมงาน WP Solution พร้อมให้บริการออกแบบและพัฒนาซอฟต์แวร์แบบครบวงจร ดูรายละเอียดบริการของเราได้ที่: www.wpsolution2017.com หรือพูดคุยปรึกษาเบื้องต้นได้ที่ Line: wisit.p