บทความนี้มาดูกันนะครับว่าจะคำนวณค่ามุม Roll และ Pitchจากเซ็นเซอร์วัดความเร่ง (accelerometer) ได้อย่างไร โดยผมขออธิบายมุม Roll Pitch Yaw ให้เข้าใจแบบง่ายๆด้วยภาพด้านล่างแล้วกันนะครับ
เนื่องจากข้อจำกัดของ accelerometer ค่า yaw ที่วัดได้จะมีความคลาดเคลื่อนสูงจนนำมาใช้ไม่ได้ โดยทั่วไปถ้าใช้แค่ accelerometer อย่างเดียวมุม yaw จะไม่ถูกนำมาใช้ ดังนั้นในบทความนี้เราจะพูดถึงแค่ roll และ pitch เท่านั้น
วิธีคำนวณ roll และ pitch นั้นจะขึ้นอยู่กับชนิดของ accelerometer ในที่นี้เราจะอ้างถึงตระกูล ADXL345 เนื่องจากมี Library ช่วยแปลงค่าสัญญาณ analog ที่อ่านได้จาก accelerometer ให้อยู่ในรูปของแรง “g” (หรือความเร่ง) ถ้าไม่ใช้ library นี้เราก็จะต้องเขียนโปรแกรมในส่วนของการแปลงนี้เองโดยใช้สูตรด้านล่าง
โดยที่ค่า Range และ resolution เป็นค่าเฉพาะของแต่ละ accelerometer เช่นสำหรับ ADXL345 ค่าทั้งสองนี้คือresolution = 10-bit และ Range = +-2 g
โดยทั่วไปค่าความเร่งที่วัดได้ในแต่ละแกน (Xg, Yg, Zg) จะมีสัญญาณรบกวนผสมอยู่ จึงควรทำการลดปริมาณสัญญาณรบกวนนี้ออกไปก่อนบ้าง โดยใช้ Low-Pass Filter ซึ่ง implement ได้ดังนี้
ค่าแอลฟา เป็นค่าคงที่ของ Lowpass filter โดยจะต้องมีค่าน้อยกว่าหรือเท่ากับ 1 ค่าแอลฟาที่เหมาะขึ้นอยู่กับระบบและรูปแบบการใช้งาน ส่วน y(k) และ x(k) คือค่าความเร่งหลังและก่อน filter ตามลำดับ y(k-1) คือค่าความเร่งที่ถูก filter ของ loop ที่แล้ว
หลังจากที่ได้ความเร่งที่ถูก filter ของทั้ง 3 แกนมา (fXg, fYg, fZg) เราสามารถคำนวณหา pitch และ roll ได้ดังนี้
ด้านล่างเป็นตัวอย่างโค๊ดที่เรียกใช้ library สองตัว คือ Wire.h และ ADXL345.h โดยใช้ Lowpass filter ลดทอนสัญญาณรบกวนออกจากค่าความเร่งที่อ่านค่าได้ในแต่ละแกน จากนั้นก็ทำการคำนวณมุม roll และ pitch ในแต่ละ loop ตามลำดับ
ไม่มีความคิดเห็น:
แสดงความคิดเห็น