ตัวกรอง Madgwick
- ผู้เสนอ
- Sebastian Madgwick
- ปี
- 2010
- ประเภท
- Filter algorithm
- กลุ่มวิธี
- Gradient Descent Filtering
ข้อความนี้เป็นการแปลด้วยเครื่องและยังไม่ได้รับการตรวจทาน อ่านต้นฉบับภาษาอังกฤษ
ใช้ทำอะไร?
ตัวกรอง Madgwick (Madgwick Filter) เป็นอัลกอริทึมการประมาณค่าทัศนคติที่มีน้ำหนักการคำนวณเบา ซึ่งหลอมรวมการวัดค่าเฉื่อย (มาตรความเร่ง, ไจโรสโคป) เข้ากับการวัดค่าสนามแม่เหล็ก (มาตรวัดสนามแม่เหล็ก) เพื่อคำนวณการวางแนวแบบควอเทอร์เนียน อัลกอริทึมนี้ถูกนำเสนอโดย Sebastian Madgwick ในปี 2010 โดยใช้วิธีการหาค่าเหมาะที่สุดแบบการไล่ระดับความชัน (gradient descent optimization) เพื่อลดความคลาดเคลื่อนระหว่างผลลัพธ์ของเซ็นเซอร์ที่วัดได้และที่คาดการณ์ไว้ ทำให้ได้ค่าประมาณทัศนคติที่แม่นยำและไม่มีการเลื่อนไหล (drift-free) บนระบบฝังตัว (embedded systems) โดยมีต้นทุนการคำนวณน้อยที่สุด ตัวกรอง Madgwick เป็นที่แพร่หลายในอุปกรณ์อิเล็กทรอนิกส์สำหรับผู้บริโภค, หุ่นยนต์, และระบบการบินและอวกาศในปัจจุบัน
แนวคิดเบื้องหลังวิธีการ
แนวคิดคือการหาควอเทอร์เนียน (การวางแนว) ที่อธิบายการวัดค่าจากเซ็นเซอร์ที่สังเกตได้ดีที่สุด มาตรความเร่งและเข็มทิศบอกเราว่าแรงโน้มถ่วงและทิศเหนือแม่เหล็กอยู่ที่ใด; ไจโรสโคปบอกเราว่าเรากำลังหมุนเร็วแค่ไหน อัลกอริทึมจะคำนวณทิศทาง (การไล่ระดับความชัน) ของการปรับทัศนคติที่ทำให้เวกเตอร์ที่ประมาณค่าและที่วัดได้เข้าใกล้กันมากขึ้น จากนั้นจะก้าวไปในทิศทางนั้นเล็กน้อย การทำซ้ำหลายครั้งต่อวินาทีนี้จะสร้างการประมาณค่าทัศนคติที่ราบรื่นและเป็นเวลาจริงซึ่งไม่มีการเลื่อนไหล
ประวัติศาสตร์
Sebastian Madgwick พัฒนาอัลกอริทึมนี้ประมาณปี 2010 ในขณะที่ทำการวิจัยเกี่ยวกับการจับการเคลื่อนไหวและการวิเคราะห์การเคลื่อนไหวของมนุษย์ อัลกอริทึมนี้ได้รับการตีพิมพ์ใน IEEE International Conference on Rehabilitation Robotics ในปี 2011 และเผยแพร่เป็นรหัสโอเพนซอร์ส ได้รับการยอมรับอย่างรวดเร็วในชุมชนหุ่นยนต์ DIY และนักสร้างสรรค์เนื่องจากความเรียบง่ายและประสิทธิภาพ บริษัทอิเล็กทรอนิกส์สำหรับผู้บริโภครายใหญ่ (สมาร์ทโฟน, ชุดหูฟัง VR, เครื่องติดตามการออกกำลังกาย) ได้นำรุ่นต่างๆ ไปใช้ ตัวกรอง Madgwick ยังคงเป็นที่นิยมในระบบฝังตัวและการศึกษา โดยมักจะทำหน้าที่เป็นบทนำสู่การหลอมรวมเซ็นเซอร์สำหรับนักเรียน
ควรใช้เมื่อใด
ใช้ตัวกรอง Madgwick เมื่อคุณต้องการการประมาณค่าทัศนคติแบบเรียลไทม์ที่มีน้ำหนักเบาบนอุปกรณ์ที่มีทรัพยากรจำกัด (ไมโครคอนโทรลเลอร์, โทรศัพท์มือถือ, อุปกรณ์สวมใส่) เหมาะสำหรับแอปพลิเคชันที่ความซับซ้อนของตัวกรอง Kalman Filter มากเกินความจำเป็น มีประสิทธิภาพทั้งในร่มและกลางแจ้ง ในยานพาหนะ, โดรน, และอุปกรณ์สวมใส่ ใช้งานเมื่อคุณมี IMU แบบ 9-DOF และต้องการหลีกเลี่ยงการปรับพารามิเตอร์ตัวกรอง Kalman Filter หลายตัว ใช้สำหรับอุปกรณ์อิเล็กทรอนิกส์สำหรับผู้บริโภค, หุ่นยนต์สำหรับงานอดิเรก, และระบบควบคุมแบบเรียลไทม์
ข้อจำกัดและข้อควรพิจารณา
- เป็นแบบฮิวริสติก ไม่ใช่แบบเหมาะสมที่สุด; ไม่ได้ลดฟังก์ชันต้นทุนที่เป็นทางการใดๆ (ไม่เหมือนกับตัวกรอง Kalman Filter)
- ประสิทธิภาพขึ้นอยู่กับ beta (ขนาดก้าว) อย่างมาก; ไม่มีการปรับตัวอัตโนมัติหากคุณภาพเซ็นเซอร์หรือการเคลื่อนที่เปลี่ยนแปลง
- การรบกวนของมาตรวัดสนามแม่เหล็กอาจทำให้ทิศทางผิดพลาดได้; ต้องมีการสอบเทียบด้วยตนเอง; ไม่ตรวจจับหรือปฏิเสธการวัดค่าที่ไม่ดีโดยอัตโนมัติ
- ความเร่งขนาดใหญ่ (ในรุ่น MARG) ทำให้เกิดข้อผิดพลาดชั่วคราวเนื่องจากมาตรความเร่งไม่ตรงกับแรงโน้มถ่วง; การกู้คืนของตัวกรองเป็นแบบฮิวริสติก ไม่ใช่ตามหลักการ
เงื่อนไขจากแหล่งที่มาไม่สามารถใช้แทนการทบทวนทางวิทยาศาสตร์ที่เป็นอิสระหรือการตรวจสอบโดยเอนจินวิเคราะห์ได้
จุดแข็ง
- ต้นทุนการคำนวณน้อยที่สุด; ถูกนำไปใช้ในโค้ดน้อยกว่า 100 บรรทัด; ทำงานบนไมโครคอนโทรลเลอร์และโปรเซสเซอร์มือถือ
- ไม่จำเป็นต้องใช้เมทริกซ์หรือพีชคณิตเชิงเส้นที่ซับซ้อน; ใช้เพียงพีชคณิตควอเทอร์เนียนและผลคูณจุด
- มีรุ่นสำหรับ IMU เท่านั้น (6-DOF) และ MARG (9-DOF); สามารถปรับให้เข้ากับเซ็นเซอร์ที่มีอยู่ได้
- พารามิเตอร์การปรับแต่งเดี่ยว (beta) ทำให้ง่ายต่อการปรับแต่ง; ไม่มีเมทริกซ์ความแปรปรวนร่วมหรือการผกผันเมทริกซ์
การประยุกต์ใช้
- การติดตามการเคลื่อนไหวของสมาร์ทโฟนและอุปกรณ์สวมใส่: ช่วยให้สามารถจดจำท่าทางเคลื่อนไหวและความจริงเสริมได้โดยไม่ต้องใช้การคำนวณมาก
- การรักษาเสถียรภาพของควอดคอปเตอร์และโดรน: ให้ค่าประมาณ roll, pitch, yaw สำหรับการควบคุมการบินบนระบบขับเคลื่อนอัตโนมัติที่มีทรัพยากรจำกัด
- การติดตามการเคลื่อนไหวของชุดหูฟังเสมือนจริง (VR): การวางแนวศีรษะแบบเรียลไทม์สำหรับการแสดงผลที่สมจริง
- การจับการเคลื่อนไหวและแอนิเมชันมนุษย์: ติดตามการวางแนวส่วนของร่างกายสำหรับการวิเคราะห์การเดินและแอนิเมชัน
- หุ่นยนต์และโดรนสำหรับงานอดิเรก: ทางเลือกที่มีน้ำหนักเบาสำหรับตัวกรอง Kalman Filters สำหรับการประมาณค่าทัศนคติบน Arduino, Raspberry Pi
วิธีการทำงาน
วิธีการทำงาน
รวบรวมการวัดค่าจาก IMU แบบ 9-DOF: มาตรความเร่ง 3 แกน, ไจโรสโคป 3 แกน, และมาตรวัดสนามแม่เหล็ก 3 แกน ที่ความถี่ 50+ Hz
ปรับเวกเตอร์มาตรความเร่งและมาตรวัดสนามแม่เหล็กให้เป็นเวกเตอร์หนึ่งหน่วย; เก็บเวกเตอร์อัตราดิบของไจโรสโคป; ใช้การสอบเทียบเซ็นเซอร์ที่ทราบ (ค่าความคลาดเคลื่อน, ตัวประกอบมาตราส่วน) หากมี
คำนวณการไล่ระดับความชันของความคลาดเคลื่อนในการวัดเทียบกับควอเทอร์เนียน; ปรับควอเทอร์เนียนไปในทิศทางที่ลดความคลาดเคลื่อน (gradient descent); รวมอัตราไจโรสโคปเพื่อทำนายวิวัฒนาการของควอเทอร์เนียน
สูตร
\nabla f = \frac{\partial f}{\partial \vec{q}}ผสมผสานการแก้ไขแบบการไล่ระดับความชันเข้ากับการรวมไจโรสโคปผ่านพารามิเตอร์เดี่ยว (beta) ที่ควบคุมความแรงของการแก้ไข; ส่งออกควอเทอร์เนียนในการอัปเดตแต่ละครั้ง
แปลงควอเทอร์เนียนเป็นมุมออยเลอร์ (roll, pitch, yaw) หรือใช้ควอเทอร์เนียนโดยตรง; รายงานทัศนคติ; ประเมินความเชื่อมั่นตามความน่าเชื่อถือของเซ็นเซอร์และสถานะการเคลื่อนที่
ข้อผิดพลาดที่พบบ่อย
- การเลือกค่า beta ที่สูงเกินไปทำให้เกิดการแกว่งอย่างรวดเร็วและการลู่ออก; ต่ำเกินไปทำให้การตอบสนองช้า; ช่วงปกติคือ 0.01–0.1 ซึ่งปรับแต่งโดยการทดลอง
- การละเลยการสอบเทียบมาตรวัดสนามแม่เหล็ก; การบิดเบือนแบบ soft iron และ hard iron อาจทำให้เกิดข้อผิดพลาดของทิศทาง; การสอบเทียบมีความสำคัญอย่างยิ่งต่อค่า yaw ที่แม่นยำ
- การสมมติว่ามาตรความเร่งถูกต้องเสมอ; ในระหว่างการเร่งความเร็วที่รุนแรง มาตรความเร่งไม่สามารถใช้สำหรับการประมาณค่า roll/pitch ได้; อัลกอริทึมบางตัวจะตรวจจับและละเลยมาตรความเร่งในระหว่างการเคลื่อนที่
- การใช้งานโดยไม่มีการเริ่มต้น; อัลกอริทึมจะลู่เข้าภายในไม่กี่วินาที แต่ควรเริ่มต้นด้วยยานพาหนะที่หยุดนิ่ง (การเริ่มต้นแบบคงที่)
คำถามที่พบบ่อย
ฉันจะเลือกพารามิเตอร์ beta ได้อย่างไร?
Beta ควบคุมอัตราการแก้ไขแบบการไล่ระดับความชัน เริ่มต้นด้วย beta = 0.041 (ค่าที่ Madgwick ใช้) หากตัวกรองทำงานช้า (ตอบสนองต่อการเคลื่อนไหวช้า) ให้เพิ่ม beta (สูงสุด 0.1–0.5) หากตัวกรองแกว่งหรือเลื่อนไหล ให้ลด beta (ลงไปถึง 0.01) ปรับแต่งโดยการทดลองโดยสังเกตการตอบสนองของทัศนคติกับการเคลื่อนไหวที่ทราบ
ความแตกต่างระหว่างรุ่น IMU (6-DOF) และ MARG (9-DOF) คืออะไร?
รุ่น IMU ใช้มาตรความเร่งและไจโรสโคปเท่านั้น; ให้ค่า roll และ pitch แต่ yaw จะเลื่อนไหลหากไม่มีการอ้างอิง รุ่น MARG เพิ่มมาตรวัดสนามแม่เหล็กสำหรับค่า yaw สัมบูรณ์; ให้ทัศนคติ 3-DOF ที่เสถียร แต่ไวต่อการบิดเบือนของสนามแม่เหล็ก ใช้ IMU สำหรับการบินระยะสั้น; ใช้ MARG สำหรับการนำทางแบบอยู่กับที่หรือช้า
ฉันสามารถรันตัวกรอง Madgwick บนไมโครคอนโทรลเลอร์ 32 บิตได้หรือไม่?
ได้ อัลกอริทึมมีประสิทธิภาพมาก; ARM Cortex-M4 สามารถรันได้ที่ 200+ Hz แม้แต่ไมโครคอนโทรลเลอร์ 8 บิต (ที่มีไลบรารีจุดลอยตัว) ก็สามารถจัดการได้ที่ 50+ Hz ประสิทธิภาพขึ้นอยู่กับอัตรานาฬิกาและการรองรับจุดลอยตัว ด้วยเลขคณิตแบบจุดคงที่ จะสามารถปรับขนาดไปยังโปรเซสเซอร์ที่ช้ากว่ามากได้
ฉันจะตรวจจับการรบกวนของสนามแม่เหล็กได้อย่างไร?
ตรวจสอบขนาดของมาตรวัดสนามแม่เหล็ก; สนามแม่เหล็กโลกมีค่าประมาณ 50 ไมโครเทสลา; การเบี่ยงเบนที่มากขึ้นบ่งชี้ถึงการบิดเบือน เปรียบเทียบค่าทิศทางเข็มทิศ (จากมาตรวัดสนามแม่เหล็ก) กับค่าทิศทางจากการรวมไจโร; ความไม่สอดคล้องกันอย่างมากบ่งชี้ถึงการรบกวน การบรรเทา: เพิกเฉยการอัปเดตมาตรวัดสนามแม่เหล็กชั่วคราวในระหว่างการรบกวน หรือลดน้ำหนักของมัน (ลด beta_m ในบางรุ่น)
วิธีการที่เกี่ยวข้อง
แหล่งอ้างอิง
Madgwick, S. O. H., Harrison, A. J. L., & Vaidyanathan, R. (2011). Estimation of IMU and MARG orientation using a gradient descent algorithm. IEEE International Conference on Rehabilitation Robotics (ICORR), 1–7.
เปิดเอกสารอ้างอิงสิ่งพิมพ์คัดลอกแล้วMadgwick, S. O. H. (2010). An efficient orientation filter for inertial and inertial/magnetic sensor arrays. Report x-io Technologies, University of Bristol, UK.
เปิดเอกสารอ้างอิงสิ่งพิมพ์คัดลอกแล้วSabatini, A. M. (2006). Quaternion-based extended Kalman filter for determining orientation by inertial and magnetic sensing. IEEE Transactions on Biomedical Engineering, 53(7), 1346–1356.
เปิดเอกสารอ้างอิง DOIคัดลอกแล้ว