Arduino Uno Wifi Rev2 (คำนวนมุม Roll และ Pitch จากค่าความเร่งที่วัดได้จาก Accelerometer)

Orientation หรือบางครั้งก็เรียกว่า Attitude คือตำแหน่งการวางตัวของวัตถุในระนาบ 3  มิติ โดยนิยมบอกด้วยมุม roll, pitch และ yaw ที่มีหน่วยเป็น องศา (degree) หรือ เรเดียน (radian) (ดูภาพที่ 1 ประกอบ)

โดยมุม roll pitch และ yaw สามารถคำนวนได้จากค่า ความเร่ง เพียงอย่างเดียว หรือ ใช้ทั้งค่าความเร่งและค่าความเร็วเชิงมุมนำมาผสม (fusion) กันก็ได้ (บทความถัดไปจะอธิบายกรณี fusion) ทั้งนี้การทำ fusion จะได้ค่า roll pitch yaw ที่เสถียรกว่าแบบใช้ค่าความเร่งที่วัดได้จาก accelerometer อย่างเดียว

ในบทความนี้จะใช้ค่าความเร่งของ 3 แกน (x, y, z) ที่วัดได้จาก accelerometer เพียงอย่างเดียวมาคำนวนมุม pitch และ roll

หมายเหตุ: เนื่องจากข้อจำกัดของ accelerometer ค่า yaw ที่วัดได้จะมีความคลาดเคลื่อนสูงจนนำมาใช้ไม่ได้ โดยทั่วไปถ้าใช้แค่ค่าความเร่งที่วัดได้จาก accelerometer เพียงอย่างเดียวมุม yaw จะไม่ถูกนำมาใช้ ดังนั้นในบทความนี้เราจะพูดถึงแค่ roll และ pitch เท่านั้น

โดยธรมมชาติของ aceelerometer ค่าความเร่งที่วัดได้ในแต่ละแกน (Xg, Yg, Zg) จะมีสัญญาณรบกวนผสมอยู่ จึงควรทำการลดปริมาณสัญญาณรบกวนนี้ โดยใช้ Low-Pass Filter ซึ่ง implement แยกแต่ละแกน x, y และ z ได้ดังนี้

โดย

y(k) คือค่าที่ถูกฟิลเตอร์แล้ว ณ เวลาปัจจุบัน k (loop ปัจจุบัน)
y(k-1) คือค่าก่อนถูกฟลิเตอร์ ณ เวลาก่อนหน้า k-1 (loop ที่แล้ว)
x(k) คือค่าความเร่ง ณ เวลาปัจจุบัน k ที่วัดได้ (loop ปัจจุบัน)
alpha คือค่าคงที่ของ lowpass filter มีค่าอยู่ในช่วง [0-1] และค่า default ส่วนมากจะใช้เป็น 0.5 แต่ทั้งนี้สามารถปรับเปลี่ยนตามความเหมาะสม

หลังจากที่ได้ความเร่งของทั้ง 3 แกน ได้ถูกฟิลเตอร์แล้ว เราสามารถคำนวณหา pitch และ roll ได้ดังสมการนี้

ซึ่งหากใช้บอร์ด Arduino Uno WiFi Rev2 ที่มี accelerometer บนบอร์ดอยู่แล้วการ implement สมการข้างต้นสามารถทำได้อย่างรวดเร็ว (ดูบทความก่อนหน้าสำหรับการติดตั้งบอร์ด) ดังนี้


บทความ โดย Pink Panther