ฉันมีบอร์ด Arduino ที่มีเซ็นเซอร์อิสระ 9 องศาซึ่งฉันต้องกำหนดระดับเสียงเอียงและหมุนของบอร์ด

นี่คือตัวอย่างของชุดข้อมูลหนึ่งชุดจากเซ็นเซอร์ 9-DOF:

มาตรความเร่ง (m / s)

• = -5,85$\text{Acc}_{X}$
• = 1,46$\text{Acc}_{Y}$
• = 17,98$\text{Acc}_{Z}$

Gyroscope (RPM)

• = 35,14$\text{Gyr}_{X}$
• = -40,22$\text{Gyr}_{Y}$
• = -9,86$\text{Gyr}_{Z}$

Magnetometer (เกาส์)

• = 0,18$\text{Mag}_{X}$
• = -0,04$\text{Mag}_{Y}$
• = -0,15$\text{Mag}_{Z}$

ฉันจะคำนวณ pitch, yaw และ roll จากข้อมูลเหล่านี้ได้อย่างไร

Pitch, roll และ yaw ถูกกำหนดให้เป็นการหมุนรอบแกน X, Y และ Z ด้านล่างเป็นภาพเพื่อแสดงความหมาย

ในโครงการก่อนหน้านี้ฉันใช้ ADXL345 Accelerometer จากอุปกรณ์อะนาล็อกเพื่อคำนวณม้วนและพิช ด้านล่างนี้คือสมการที่ใช้คำนวณการหมุนและพิช ฉันได้สร้างซอร์สโค้ดบางส่วนเพื่อให้สาธารณชนใช้งานได้แล้ว

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

รหัสที่สมบูรณ์สามารถพบได้ที่นี่

ยึดตามคำจำกัดความข้างต้น

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

หมายเหตุ: M_PI = 3.14159265358979323846 เป็นค่าคงที่ที่กำหนดใน math.h

ด้านล่างนี้เป็นข้อมูลอ้างอิงบางส่วนรวมถึงซอร์สโค้ดพื้นฐานของ Arduino ที่อาจช่วยคุณได้

อ้างอิง:

• การเปลี่ยนแปลงเที่ยวบิน
• Beaglebone รหัสแหล่ง ADXL345 สีดำ
• Adxl345 Beaglebone ดำสาธิต QT ในตัว
• Arduino IMU: Pitch & Roll จากมาตรความเร่ง

ดังนั้นคำตอบอีกต่อไปของฉันด้านล่างจะถือว่าบอร์ดจะได้รับการเร่งความเร็วและในช่วงเวลานี้คุณยังจำเป็นต้องวัดระยะห่างระหว่างการหมุนและการหันเหของคุณภายในระยะเวลาอันสั้น หากคณะกรรมการจะอยู่กับที่ทุกการวัดคำตอบของ Mahendra Gunawardena จะทำงานได้อย่างสมบูรณ์แบบสำหรับคุณ หากสิ่งนี้กำลังเข้าสู่อุปกรณ์อย่างเช่นเซกเวย์หรือโมเดลเครื่องบินหรือมัลติมอเตอร์หรืออะไรก็ตามที่เคลื่อนที่ไปมาคุณอาจต้องการอ่านต่อไป โพสต์นี้เกี่ยวกับวิธีการใช้เซ็นเซอร์ทั้งสามตัวแม้ว่าวิธีการที่เรียกว่าฟิวชั่นเซ็นเซอร์ การรวมเซ็นเซอร์ช่วยให้คุณได้รับจุดแข็งของแต่ละเซ็นเซอร์และลดผลกระทบจากจุดอ่อนของเซ็นเซอร์แต่ละตัว

ลักษณะและพื้นหลังของเซ็นเซอร์

ก่อนอื่นให้ทำความเข้าใจว่ามาตรวัดความเร่งจะวัดแรงทั้งหมดที่ใช้กับมันไม่ใช่แค่แรงโน้มถ่วง ดังนั้นในโลกที่สมบูรณ์แบบที่มีมาตรวัดความเร่งในตำแหน่งที่ไม่มีการสั่นสะเทือนคุณสามารถกำหนดได้อย่างสมบูรณ์แบบว่าการใช้ตรีโกณมิติพื้นฐานตามที่แสดงโดยคำตอบของ Mahendra Gunawardena อย่างไรก็ตามเนื่องจาก accelerometer จะรับแรงทั้งหมดการสั่นสะเทือนใด ๆ จะส่งผลให้เกิดเสียง ควรสังเกตว่าหากคณะกรรมการเร่งคุณไม่สามารถใช้ตรีโกณมิติอย่างง่ายได้เพราะแรงที่ accelerometer รายงานนั้นไม่เพียง แต่แรงโน้มถ่วงของโลก แต่ยังเป็นแรงที่ทำให้คุณเร่งความเร็วด้วย

แมกเนโตมิเตอร์นั้นตรงไปตรงมามากกว่านั้นก็คือ accelerometer การเคลื่อนไหวจะไม่ทำให้เกิดปัญหา แต่สิ่งต่าง ๆ เช่นเหล็กและแม่เหล็กอื่น ๆ จะจบลงที่มีผลต่อการส่งออกของคุณ หากแหล่งที่มาที่ก่อให้เกิดสัญญาณรบกวนนี้คงที่ไม่ใช่เรื่องยากที่จะจัดการ แต่ถ้าแหล่งเหล่านี้ไม่คงที่มันจะสร้างเสียงจำนวนมากที่เป็นปัญหาในการลบ

ในบรรดาเซ็นเซอร์ทั้งสามตัวไจโรสโคปสามารถพิสูจน์ได้ว่าเชื่อถือได้มากที่สุดและโดยปกติแล้วพวกเขาจะวัดความเร็วในการหมุนได้ดีมาก มันไม่ได้รับผลกระทบจากสิ่งต่าง ๆ เช่นแหล่งเหล็กและการเร่งความเร็วโดยทั่วไปจะไม่มีผลกระทบต่อความสามารถในการวัดความเร็วรอบการหมุน พวกเขาทำงานได้ดีมากในการรายงานความเร็วที่อุปกรณ์หันมาอย่างไรก็ตามเนื่องจากคุณกำลังมองหามุมที่แน่นอนคุณต้องรวมความเร็วเพื่อให้ได้ตำแหน่ง การทำเช่นนี้จะเป็นการเพิ่มข้อผิดพลาดของการวัดครั้งล่าสุดให้กับข้อผิดพลาดของการวัดใหม่เนื่องจากการรวมกันนั้นเป็นผลรวมของค่าในช่วงแม้ว่าข้อผิดพลาดสำหรับการวัดเพียงครั้งเดียวจะมีค่าเพียง 0.01 องศาต่อวินาที สามารถปิดได้ 1 องศาโดยการวัด 1,000 ครั้งคุณสามารถปิดได้ 10 องศา หากคุณทำการวัดหลายร้อยครั้งต่อวินาที คุณสามารถเห็นสิ่งนี้ทำให้เกิดปัญหา โดยทั่วไปเรียกว่าไจโรดริฟท์

เซ็นเซอร์ฟิวชั่น

ตอนนี้ความงามของการมีเซ็นเซอร์ทั้งหมดนี้ทำงานร่วมกันคือคุณสามารถใช้ข้อมูลจากมาตรวัดความเร่งและมาตรวัดแม่เหล็กเพื่อยกเลิกการเลื่อนไจโร สิ่งนี้จะช่วยให้คุณให้ความแม่นยำและความเร็วของไจโรโดยไม่มีข้อผิดพลาดร้ายแรงของการเลื่อนไจโร

การรวมข้อมูลจากเซ็นเซอร์สามตัวนี้สามารถทำได้มากกว่าหนึ่งวิธีฉันจะพูดถึงการใช้ตัวกรองเสริมเนื่องจากมันจะง่ายกว่าตัวกรองคาลมานและตัวกรองคาลมานจะกินทรัพยากรมากขึ้นในระบบฝังตัว บ่อยครั้งที่ตัวกรองเสริมนั้นดีพอง่ายต่อการปรับใช้ (สมมติว่าคุณไม่ได้ใช้ไลบรารีที่สร้างไว้ล่วงหน้า) และช่วยให้คุณสามารถประมวลผลข้อมูลได้เร็วขึ้น

ตอนนี้เข้าสู่กระบวนการ ขั้นตอนแรกที่คุณต้องทำคือการรวมเอาท์พุทไจโรสโคปเพื่อแปลงความเร็วเชิงมุมเป็นตำแหน่งเชิงมุม คุณมักจะต้องใช้ตัวกรองความถี่ต่ำผ่าน accelerometer และ magnetometer เพื่อจัดการกับเสียงรบกวนในเอาต์พุต ตัวกรอง FIR อย่างง่ายอย่างที่แสดงด้านล่างใช้งานได้ที่นี่ ด้วยตรีโกณมิติบางตัวคุณสามารถหาระยะพิทช์และหมุนด้วยมาตรวัดความเร่งและหันเหด้วยแมกนิโทมิเตอร์

filteredData = (1-weight)*filteredData + weight*newData

น้ำหนักเป็นค่าคงที่ที่สามารถปรับได้ขึ้นอยู่กับว่าคุณต้องจัดการกับเสียงรบกวนมากเท่าใดเสียงยิ่งมีค่าน้ำหนักมากขึ้นเท่าใด ตอนนี้การรวมข้อมูลจากเซ็นเซอร์สามารถทำได้โดยบรรทัดของรหัสต่อไปนี้

fusedData = (1-weight)*gyroData + weight*accelMagData

ควรสังเกตว่าข้อมูลนั้นเป็นเวกเตอร์ของพิทช์ม้วนและหันเห คุณสามารถใช้ตัวแปรสามตัวในการทำเช่นนี้แทนการใช้อาร์เรย์ถ้าคุณต้องการ สำหรับการคำนวณนี้ไจโรให้ตำแหน่งเป็นองศาในพิทช์, หมุนและหันเห, สนามแม่เหล็กให้มุมสำหรับหันเหขณะที่ accelerometer ให้ตัวเลขของตัวเองสำหรับพิทช์และม้วน

หากคุณยังต้องการข้อมูลเพิ่มเติมคุณสามารถ google "ฟิวชั่นเซ็นเซอร์พร้อมตัวกรองเสริม" มีบทความมากมายเกี่ยวกับเรื่องนี้

จากข้อมูลเซ็นเซอร์คันเร่งคุณสามารถคำนวณระยะพิทช์แอนด์โรลเท่านั้น เอกสารด้านล่างจาก Freescale อธิบายด้วยข้อมูลมากมายที่คุณต้องการ:

AN3461 - การตรวจจับเอียงโดยใช้มาตรวัดความเร่งสามแกน

ตามคำพูดของเอกสาร

ซึ่งเท่ากับ:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

แน่นอนว่าผลลัพธ์จะเกิดขึ้นเฉพาะเมื่อมีการหมุนเกิดขึ้นตามลำดับที่ระบุ (Rxyz):

1. $\phi$
2. $\theta$
3. $\psi$

$R_{xyz}$$\psi$