มาตรความเร่งและ Gyro บทช่วยสอน: 3 ขั้นตอน

2024 ผู้เขียน: John Day | [email protected]. แก้ไขล่าสุด: 2024-01-30 13:09

บทนำ

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

เราจะครอบคลุม:

• มาตรความเร่งวัดอะไร?
• ไจโรสโคป (หรือที่เรียกว่าไจโร) วัดอะไร?
• วิธีแปลงค่าที่อ่านได้จากแอนะล็อกเป็นดิจิทัล (ADC) ที่คุณได้รับจากเซ็นเซอร์เหล่านี้เป็นหน่วยทางกายภาพ (ซึ่งจะเป็น g สำหรับมาตรความเร่ง, องศา/วินาทีสำหรับไจโรสโคป)
• วิธีรวมค่าความเร่งและค่าไจโรสโคปเข้าด้วยกันเพื่อให้ได้ข้อมูลที่แม่นยำเกี่ยวกับความเอียงของอุปกรณ์ที่สัมพันธ์กับระนาบพื้น

ตลอดทั้งบทความ ฉันจะพยายามใช้คณิตศาสตร์ให้น้อยที่สุด หากคุณรู้ว่าไซน์/โคไซน์/แทนเจนต์คืออะไร คุณควรจะสามารถเข้าใจและใช้แนวคิดเหล่านี้ในโครงการของคุณได้ ไม่ว่าคุณจะใช้แพลตฟอร์มใด: Arduino, ใบพัด, แสตมป์พื้นฐาน, ชิป Atmel, ไมโครชิป PIC เป็นต้น

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

ฉันจะใช้เป็นตัวอย่างหน่วย IMU ใหม่ นั่นคือ Acc_Gyro Accelerometer + Gyro IMU เราจะใช้พารามิเตอร์ของอุปกรณ์นี้ในตัวอย่างด้านล่าง เครื่องนี้เป็นอุปกรณ์ที่ดีในการเริ่มต้นเพราะประกอบด้วย 2 อุปกรณ์:

- LIS331AL (เอกสารข้อมูล) - มาตรความเร่ง 2G แบบสามแกน - LPR550AL (เอกสารข้อมูล) - ระยะพิทช์และม้วนสองแกน, ไจโรสโคป 500 องศา/วินาที

ร่วมกันเป็นตัวแทนของหน่วยวัดแรงเฉื่อยอิสระ 5 องศา ตอนนี้เป็นชื่อแฟนซี! อย่างไรก็ตาม เบื้องหลังชื่อแฟนซีเป็นอุปกรณ์รวมที่มีประโยชน์มาก ซึ่งเราจะอธิบายและอธิบายโดยละเอียดในคู่มือนี้

ขั้นตอนที่ 1: มาตรความเร่ง

เพื่อให้เข้าใจหน่วยนี้ เราจะเริ่มด้วยมาตรความเร่ง เมื่อนึกถึงมาตรความเร่ง มักจะเป็นประโยชน์ในการสร้างภาพกล่องที่มีรูปร่างเป็นลูกบาศก์ที่มีลูกบอลอยู่ข้างใน คุณอาจจินตนาการอย่างอื่นเช่น คุกกี้ หรือ โดนัท แต่ฉันจะจินตนาการถึงลูกบอล:

หากเรานำกล่องนี้ไปไว้ในที่ที่ไม่มีสนามโน้มถ่วง หรือถ้าไม่มีสนามอื่นที่อาจส่งผลต่อตำแหน่งของลูกบอล ลูกบอลก็จะลอยอยู่ตรงกลางกล่อง คุณสามารถจินตนาการได้ว่ากล่องนี้อยู่ในอวกาศที่ห่างไกลจากวัตถุในจักรวาล หรือหากสถานที่นั้นหายาก ลองนึกภาพยานอวกาศที่โคจรรอบโลกซึ่งทุกอย่างอยู่ในสภาพไร้น้ำหนัก จากภาพด้านบน คุณจะเห็นว่าเรากำหนดผนังคู่หนึ่งให้กับแต่ละแกน (เราลบผนัง Y+ เพื่อให้เราสามารถมองเข้าไปในกล่องได้) ลองนึกภาพว่าผนังแต่ละด้านไวต่อแรงกด ถ้าเราขยับกล่องไปทางซ้ายกะทันหัน (เราเร่งมันด้วยความเร่ง 1g = 9.8m/s^2) ลูกบอลจะชนกำแพง X- จากนั้นเราวัดแรงกดที่ลูกบอลนำไปใช้กับผนังและส่งออกค่า -1g บนแกน X

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

ถ้าเรานำแบบจำลองของเราไปวางบนพื้นโลก ลูกบอลจะตกลงบนกำแพง Z และจะใช้แรง 1g กับผนังด้านล่าง ดังแสดงในภาพด้านล่าง:

ในกรณีนี้ กล่องไม่เคลื่อนที่ แต่เรายังคงอ่านค่า -1g บนแกน Z แรงกดที่ลูกบอลกระทบกับผนังเกิดจากแรงโน้มถ่วง ในทางทฤษฎี มันอาจเป็นแรงประเภทอื่น ตัวอย่างเช่น หากคุณนึกภาพว่าลูกบอลของเราเป็นโลหะ การวางแม่เหล็กไว้ข้างกล่องจะทำให้ลูกบอลเคลื่อนตัวไปชนกับผนังอีกด้าน นี้ถูกกล่าวเพียงเพื่อพิสูจน์ว่าในสาระสำคัญมาตรความเร่งวัดแรงไม่เร่ง มันเพิ่งเกิดขึ้นที่การเร่งความเร็วทำให้เกิดแรงเฉื่อยที่จับโดยกลไกการตรวจจับแรงของมาตรความเร่ง

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

จนถึงตอนนี้ เราได้วิเคราะห์เอาท์พุตของมาตรความเร่งบนแกนเดียว และนี่คือทั้งหมดที่คุณจะได้รับจากมาตรความเร่งแบบแกนเดียว ค่าที่แท้จริงของมาตรความเร่งแบบสามแกนนั้นมาจากข้อเท็จจริงที่พวกมันสามารถตรวจจับแรงเฉื่อยบนทั้งสามแกนได้ กลับไปที่รูปแบบกล่องของเราแล้วหมุนกล่องไปทางขวา 45 องศา ตอนนี้ลูกบอลจะแตะ 2 กำแพง: Z- และ X- ตามที่แสดงในภาพด้านล่าง:

ค่า 0.71 ไม่ได้กำหนดโดยพลการ แท้จริงแล้วเป็นค่าประมาณของ SQRT(1/2) สิ่งนี้จะชัดเจนยิ่งขึ้นเมื่อเราแนะนำรุ่นถัดไปสำหรับมาตรความเร่ง

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

โปรดดูที่โมเดลด้านบน ฉันรักษาสีของแกนไว้ เพื่อให้คุณสามารถเปลี่ยนจิตใจจากรุ่นก่อนหน้าเป็นแบบใหม่ได้ ลองนึกภาพว่าแต่ละแกนในโมเดลใหม่ตั้งฉากกับใบหน้าตามลำดับของกล่องในรุ่นก่อนหน้า เวกเตอร์ R คือเวกเตอร์แรงที่มาตรวัดความเร่งกำลังวัด (อาจเป็นแรงโน้มถ่วงหรือแรงเฉื่อยจากตัวอย่างด้านบนหรือทั้งสองอย่างรวมกัน) Rx, Ry, Rz เป็นการฉายภาพของเวกเตอร์ R บนแกน X, Y, Z โปรดสังเกตความสัมพันธ์ต่อไปนี้:

R^2 = Rx^2 + Ry^2 + Rz^2 (สมการที่ 1)

ซึ่งโดยพื้นฐานแล้วเทียบเท่ากับทฤษฎีบทพีทาโกรัสในแบบ 3 มิติ

จำไว้ว่าก่อนหน้านี้เล็กน้อย ฉันบอกคุณว่าค่าของ SQRT(1/2) ~ 0.71 นั้นไม่ได้สุ่ม หากคุณนำมันมาใส่ในสูตรข้างต้น หลังจากที่ระลึกได้ว่าแรงโน้มถ่วงของเรามีค่าเท่ากับ 1 ก. เราก็สามารถยืนยันได้ว่า:

1^2 = (-SQRT(1/2))^2 + 0 ^2 + (-SQRT(1/2))^2

เพียงแค่แทนที่ R=1, Rx = -SQRT(1/2), Ry = 0, Rz = -SQRT(1/2) ในสมการที่ 1

หลังจากคำนำของทฤษฎีมายาวนาน เราก็เข้าใกล้มาตรความเร่งในชีวิตจริงมากขึ้น ค่า Rx, Ry, Rz นั้นสัมพันธ์เชิงเส้นตรงกับค่าที่มาตรวัดความเร่งในชีวิตจริงของคุณจะส่งออก และคุณสามารถใช้สำหรับการคำนวณต่างๆ

ก่อนที่เราจะไปถึงที่นั่น มาพูดคุยกันเล็กน้อยเกี่ยวกับวิธีที่มาตรความเร่งส่งข้อมูลนี้ให้เรา มาตรความเร่งส่วนใหญ่จะแบ่งออกเป็นสองประเภท: ดิจิตอลและแอนะล็อก มาตรวัดความเร่งแบบดิจิตอลจะให้ข้อมูลแก่คุณโดยใช้โปรโตคอลแบบอนุกรม เช่น I2C, SPI หรือ USART ในขณะที่มาตรวัดความเร่งแบบแอนะล็อกจะส่งสัญญาณระดับแรงดันไฟฟ้าภายในช่วงที่กำหนดไว้ล่วงหน้า ซึ่งคุณต้องแปลงเป็นค่าดิจิทัลโดยใช้โมดูล ADC (ตัวแปลงแอนะล็อกเป็นดิจิทัล) ฉันจะไม่ลงรายละเอียดมากเกี่ยวกับวิธีการทำงานของ ADC ส่วนหนึ่งเนื่องจากเป็นหัวข้อที่กว้างขวางและส่วนหนึ่งเป็นเพราะมันแตกต่างจากแพลตฟอร์มหนึ่งไปอีกแพลตฟอร์มหนึ่ง ไมโครคอนโทรลเลอร์บางตัวจะมีโมดูล ADC ในตัว ซึ่งบางตัวอาจต้องการส่วนประกอบภายนอกเพื่อทำการแปลง ADC ไม่ว่าคุณจะใช้โมดูล ADC ประเภทใด คุณก็จะได้ค่าในช่วงที่กำหนด ตัวอย่างเช่น โมดูล ADC 10 บิตจะส่งออกค่าในช่วง 0..1023 โปรดทราบว่า 1023 = 2^10 -1 โมดูล ADC 12 บิตจะส่งออกค่าในช่วง 0..4095 โปรดทราบว่า 4095 = 2 ^ 12-1

มาพิจารณาตัวอย่างง่ายๆ กัน สมมติว่าโมดูล 10 บิต ADC ให้ค่าต่อไปนี้แก่เราสำหรับช่องสัญญาณความเร่ง (แกน) สามช่อง:

AdcRx = 586 AdcRy = 630 AdcRz = 561

โมดูล ADC แต่ละโมดูลจะมีแรงดันอ้างอิง สมมติว่าในตัวอย่างของเราคือ 3.3V ในการแปลงค่า adc 10 บิตเป็นแรงดันไฟฟ้า เราใช้สูตรต่อไปนี้:

โวลต์Rx = AdcRx * Vref / 1023

บันทึกย่อที่นี่: สำหรับ 8 บิต ADC ตัวหารสุดท้ายจะเป็น 255 = 2 ^ 8 -1 และสำหรับตัวหารสุดท้าย ADC 12 บิตจะเป็น 4095 = 2^12 -1

การนำสูตรนี้ไปใช้กับทั้ง 3 ช่องทาง เราได้:

VoltsRx = 586 * 3.3V / 1023 =~ 1.89V (เราปัดเศษผลลัพธ์ทั้งหมดเป็นทศนิยม 2 ตำแหน่ง) VoltsRy = 630 * 3.3V / 1023 =~ 2.03V VoltsRz = 561 * 3.3V / 1023 =~ 1.81V

มาตรความเร่งแต่ละตัวมีระดับแรงดันไฟฟ้าเป็นศูนย์ คุณสามารถหาได้ในข้อมูลจำเพาะ นี่คือแรงดันไฟฟ้าที่สอดคล้องกับ 0g ในการรับค่าแรงดันไฟฟ้าที่ลงนาม เราจำเป็นต้องคำนวณการเปลี่ยนแปลงจากระดับนี้ สมมติว่าระดับแรงดัน 0g ของเราคือ VzeroG = 1.65V เราคำนวณการเปลี่ยนแปลงแรงดันไฟฟ้าจากแรงดันศูนย์-g ดังนี้::

DeltaVoltsRx = 1.89V - 1.65V = 0.24V DeltaVoltsRy = 2.03V - 1.65V = 0.38V DeltaVoltsRz = 1.81V - 1.65V = 0.16V

ตอนนี้เรามีการอ่านค่าความเร่งในหน่วย Volts ซึ่งยังไม่อยู่ในหน่วย g (9.8 m/s ^ 2) ในการแปลงขั้นสุดท้าย เราใช้ความไวของตัวตรวจวัดความเร่ง ซึ่งมักจะแสดงเป็น mV/g สมมติว่าความไวของเรา = 478.5mV/g = 0.4785V/g ค่าความไวสามารถพบได้ในข้อกำหนดของมาตรความเร่ง เพื่อให้ได้ค่าแรงสุดท้ายที่แสดงเป็น g เราใช้สูตรต่อไปนี้:

Rx = DeltaVoltsRx / ความไว

Rx = 0.24V / 0.4785V/g =~ 0.5g Ry = 0.38V / 0.4785V/g =~ 0.79g Rz = 0.16V / 0.4785V/g =~ 0.33g

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

Rx = (AdcRx * Vref / 1023 - VzeroG) / ความไว (Eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / ความไว Rz = (AdcRz * Vref / 1023 - VzeroG) / ความไว

ตอนนี้เรามีองค์ประกอบ 3 อย่างที่กำหนดเวกเตอร์แรงเฉื่อย ถ้าอุปกรณ์ไม่อยู่ภายใต้แรงอื่นนอกจากแรงโน้มถ่วง เราสามารถสันนิษฐานได้ว่านี่คือทิศทางของเวกเตอร์แรงโน้มถ่วงของเรา หากคุณต้องการคำนวณความเอียงของอุปกรณ์ที่สัมพันธ์กับพื้น คุณสามารถคำนวณมุมระหว่างเวกเตอร์นี้กับแกน Z ได้ หากคุณสนใจทิศทางเอียงต่อแกนด้วย คุณสามารถแบ่งผลลัพธ์นี้เป็น 2 องค์ประกอบ: ความเอียงบนแกน X และ Y ที่สามารถคำนวณเป็นมุมระหว่างเวกเตอร์ความโน้มถ่วงและแกน X / Y การคำนวณมุมเหล่านี้ง่ายกว่าที่คุณคิด ตอนนี้เราได้คำนวณค่าสำหรับ Rx, Ry และ Rz แล้ว กลับไปที่รุ่นของมาตรความเร่งล่าสุดของเราแล้วทำสัญลักษณ์เพิ่มเติม:

มุมที่เราสนใจคือมุมระหว่างแกน X, Y, Z และเวกเตอร์แรง R เราจะนิยามมุมเหล่านี้เป็น Axr, Ayr, Azr คุณสามารถสังเกตได้จากสามเหลี่ยมมุมฉากที่เกิดจาก R และ Rx ว่า:

cos(Axr) = Rx / R และในทำนองเดียวกัน: cos(Ayr) = Ry / R cos(Azr) = Rz / R

เราสามารถหักออกจากสมการที่ 1 ได้ว่า R = SQRT(Rx^2 + Ry^2 + Rz^2)

ตอนนี้เราสามารถหามุมของเราได้โดยใช้ฟังก์ชัน arccos() (ฟังก์ชัน inverse cos()):

Axr = arccos(Rx/R) Ayr = arccos(Ry/R) Azr = arccos(Rz/R)

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

แต่ก่อนที่เราจะทำอย่างนั้น เรามาทำสัญลักษณ์ที่มีประโยชน์กว่านี้กันก่อน:

cosX = cos(Axr) = Rx / R สบาย = cos(Ayr) = Ry / R cosZ = cos(Azr) = Rz / R

แฝดสามตัวนี้มักเรียกว่า Direction Cosine และโดยพื้นฐานแล้วหมายถึงเวกเตอร์หน่วย (เวกเตอร์ที่มีความยาว 1) ที่มีทิศทางเดียวกับเวกเตอร์ R ของเรา คุณสามารถตรวจสอบได้ง่ายๆ ว่า:

SQRT(cosX^2 + cosY^2 + cosZ^2) = 1

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

ขั้นตอนที่ 2: Gyroscope

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

ช่องไจโรสโคปแต่ละช่องจะวัดการหมุนรอบแกนใดแกนหนึ่ง ตัวอย่างเช่น ไจโรสโคป 2 แกนจะวัดการหมุนรอบ (หรือบางคนอาจพูดว่า "ประมาณ") แกน X และ Y เพื่อแสดงการหมุนนี้เป็นตัวเลข ลองทำสัญลักษณ์บางอย่าง ขั้นแรกให้กำหนด:

Rxz - เป็นการฉายภาพของเวกเตอร์แรงเฉื่อย R บนระนาบ XZ Ryz - เป็นการฉายภาพเวกเตอร์แรงเฉื่อย R บนระนาบ YZ

จากสามเหลี่ยมมุมฉากที่เกิดจาก Rxz และ Rz โดยใช้ทฤษฎีบทพีทาโกรัสเราจะได้:

Rxz^2 = Rx^2 + Rz^2 และในทำนองเดียวกัน: Ryz^2 = Ry^2 + Rz^2

โปรดทราบว่า:

R^2 = Rxz^2 + Ry^2 ซึ่งสามารถได้มาจากสมการที่ 1 และสมการข้างต้น หรือสามารถหาได้จากสามเหลี่ยมมุมฉากที่เกิดจาก R และ Ryz R^2 = Ryz^2 + Rx^2

เราจะไม่ใช้สูตรเหล่านี้ในบทความนี้ แต่ควรสังเกตความสัมพันธ์ระหว่างค่าทั้งหมดในแบบจำลองของเรา

เราจะกำหนดมุมระหว่างแกน Z และ Rxz แทน, เวกเตอร์ Ryz ดังต่อไปนี้:

Axz - คือมุมระหว่าง Rxz (การฉายภาพ R บนระนาบ XZ) และแกน Z Ayz - คือมุมระหว่าง Ryz (การฉายภาพ R บนระนาบ YZ) และแกน Z

ตอนนี้เราเข้าใกล้สิ่งที่ไจโรสโคปวัดมากขึ้นแล้ว Gyroscope วัดอัตราการเปลี่ยนแปลงของมุมที่กำหนดไว้ข้างต้น กล่าวอีกนัยหนึ่งก็จะส่งออกค่าที่เกี่ยวข้องกับอัตราการเปลี่ยนแปลงของมุมเหล่านี้เชิงเส้น เพื่ออธิบายสิ่งนี้ สมมติว่าเราได้วัดมุมการหมุนรอบแกน Y (ซึ่งจะเป็นมุม Axz) ที่เวลา t0 และเรากำหนดให้มันเป็น Axz0 ต่อไปเราจะวัดมุมนี้ในเวลาต่อมา t1 และมันคือ Axz1 อัตราการเปลี่ยนแปลงจะถูกคำนวณดังนี้:

RateAxz = (Axz1 - Axz0) / (t1 - t0).

ถ้าเราแสดง Axz เป็นองศา และเวลาเป็นวินาที ค่านี้จะแสดงเป็น deg/s นี่คือสิ่งที่เครื่องวัดการหมุนวนวัด

ในทางปฏิบัติ ไจโรสโคป (เว้นแต่จะเป็นไจโรสโคปแบบพิเศษ) แทบจะไม่ให้ค่าที่แสดงเป็นองศา/วินาที เช่นเดียวกับมาตรความเร่ง คุณจะได้ค่า ADC ที่คุณต้องแปลงเป็นองศา/วินาทีโดยใช้สูตรที่คล้ายกับสมการ 2 ที่เรากำหนดไว้สำหรับมาตรความเร่ง ขอแนะนำสูตรการแปลง ADC เป็น deg/s สำหรับไจโรสโคป (เราถือว่าเรากำลังใช้โมดูล ADC 10 บิต สำหรับ 8 บิต ADC แทนที่ 1023 ด้วย 255 สำหรับ 12 บิต ADC แทนที่ 1023 ด้วย 4095)

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / ความไว Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / ความไว

AdcGyroXZ, AdcGyroYZ - ได้มาจากโมดูล adc ของเรา และเป็นตัวแทนของช่องที่วัดการหมุนของการฉายภาพของเวกเตอร์ R ใน XZ ตามลำดับในระนาบ YZ ซึ่งเทียบเท่ากับการบอกว่าการหมุนรอบแกน Y และ X ตามลำดับ

Vref - คือแรงดันอ้างอิง ADC เราจะใช้ 3.3V ในตัวอย่างด้านล่าง VzeroRate - คือแรงดันไฟศูนย์ หรืออีกนัยหนึ่งคือแรงดันที่ไจโรสโคปส่งออกเมื่อไม่มีการหมุนใดๆ สำหรับบอร์ด Acc_Gyro มันคือ ตัวอย่างเช่น 1.23V (คุณสามารถหาค่านี้ได้ในข้อมูลจำเพาะ) ความไว - คือความไวของไจโรสโคปที่แสดงเป็น mV / (deg / s) มักเขียนเป็น mV/deg/s โดยทั่วไปจะบอกคุณว่าจะมีกี่ mV เอาต์พุตของไจโรสโคปจะเพิ่มขึ้น หากคุณเพิ่มความเร็วในการหมุนหนึ่งองศา/วินาที ความไวของบอร์ด Acc_Gyro เช่น 2mV/deg/s หรือ 0.002V/deg/s

มาดูตัวอย่างกัน สมมติว่าโมดูล ADC ของเราคืนค่าดังต่อไปนี้:

AdcGyroXZ = 571 AdcGyroXZ = 323

โดยใช้สูตรข้างต้น และใช้พารามิเตอร์ข้อมูลจำเพาะของบอร์ด Acc_Gyro เราจะได้รับ:

RateAxz = (571 * 3.3V / 1023 - 1.23V) / (0.002V/deg/s) =~ 306 deg/s RateAyz = (323 * 3.3V / 1023 - 1.23V) / (0.002V/deg/s) =~ -94 องศา/วินาที

กล่าวอีกนัยหนึ่งอุปกรณ์หมุนรอบแกน Y (หรือเราสามารถพูดได้ว่าหมุนในระนาบ XZ) ด้วยความเร็ว 306 องศา/วินาที และรอบแกน X (หรืออาจกล่าวได้ว่าหมุนในระนาบ YZ) ด้วยความเร็ว - 94 องศา/วิ โปรดทราบว่าเครื่องหมายลบหมายความว่าอุปกรณ์หมุนไปในทิศทางตรงกันข้ามจากทิศทางบวกแบบเดิม ตามแบบแผน ทิศทางการหมุนหนึ่งทิศทางเป็นบวก แผ่นข้อมูลจำเพาะของไจโรสโคปที่ดีจะแสดงให้คุณเห็นว่าทิศทางใดเป็นบวก ไม่เช่นนั้น คุณจะต้องค้นหาโดยการทดลองกับอุปกรณ์และสังเกตว่าทิศทางการหมุนใดส่งผลให้แรงดันไฟขาออกเพิ่มขึ้น วิธีนี้ทำได้ดีที่สุดโดยใช้ออสซิลโลสโคป เพราะทันทีที่คุณหยุดการหมุน แรงดันไฟฟ้าจะลดลงกลับไปที่ระดับศูนย์ หากคุณกำลังใช้มัลติมิเตอร์ คุณจะต้องรักษาอัตราการหมุนให้คงที่เป็นเวลาอย่างน้อยสองสามวินาที และสังเกตแรงดันไฟฟ้าในระหว่างการหมุนนี้ จากนั้นเปรียบเทียบกับแรงดันไฟฟ้าอัตราศูนย์ ถ้ามากกว่าแรงดันศูนย์หมายความว่าทิศทางของการหมุนเป็นบวก

ขั้นตอนที่ 3: การรวมมาตรความเร่งและไจโร

รวมทุกอย่างเข้าด้วยกัน - รวมข้อมูลมาตรความเร่งและไจโรสโคป

หากคุณกำลังอ่านบทความนี้ คุณอาจได้รับหรือกำลังวางแผนที่จะซื้ออุปกรณ์ IMU หรือบางทีคุณกำลังวางแผนที่จะสร้างอุปกรณ์จากมาตรความเร่งและไจโรสโคปที่แยกจากกัน

ขั้นตอนแรกในการใช้อุปกรณ์ IMU แบบผสมผสานที่รวมมาตรความเร่งและไจโรสโคปคือการจัดตำแหน่งระบบพิกัด วิธีที่ง่ายที่สุดคือการเลือกระบบพิกัดของมาตรความเร่งเป็นระบบพิกัดอ้างอิงของคุณ แผ่นข้อมูลมาตรความเร่งส่วนใหญ่จะแสดงทิศทางของแกน X, Y, Z ที่สัมพันธ์กับภาพของชิปหรืออุปกรณ์จริง ตัวอย่างเช่น นี่คือทิศทางของแกน X, Y, Z ตามที่แสดงในข้อกำหนดสำหรับบอร์ด Acc_Gyro:

ขั้นตอนต่อไปคือ:

ระบุเอาต์พุตของไจโรสโคปที่สอดคล้องกับ RateAxz, ค่า RateAyz ที่กล่าวถึงข้างต้น ตรวจสอบว่าเอาต์พุตเหล่านี้จำเป็นต้องกลับด้านหรือไม่เนื่องจากตำแหน่งทางกายภาพของไจโรสโคปที่สัมพันธ์กับมาตรความเร่ง

อย่าทึกทักเอาเองว่าหากไจโรสโคปมีเอาต์พุตที่มีเครื่องหมาย X หรือ Y ไจโรสโคปจะสอดคล้องกับแกนใดๆ ในระบบพิกัดความเร่ง แม้ว่าเอาต์พุตนี้จะเป็นส่วนหนึ่งของหน่วย IMU ก็ตาม วิธีที่ดีที่สุดคือการทดสอบ สมมติว่าคุณได้กำหนดตำแหน่งของไจโรสโคปให้สัมพันธ์กับมาตรความเร่งแล้ว สันนิษฐานว่าขอบไจโรและมาตรความเร่งจะขนานกัน กล่าวคือ คุณกำลังวางไจโรที่มุมคูณ 90 องศาที่สัมพันธ์กับชิปมาตรความเร่ง หากคุณได้รับบอร์ด IMU โอกาสที่พวกเขาจะได้รับการจัดตำแหน่งด้วยวิธีนี้ เราจะไม่พูดถึงในบทความนี้ โมเดลที่วางไจโรสโคปในมุมที่ไม่ปกติเมื่อเทียบกับมาตรความเร่ง (สมมติว่า 45 หรือ 30 องศา) แม้ว่านี่อาจมีประโยชน์ในบางแอปพลิเคชัน

นี่คือลำดับตัวอย่างเพื่อกำหนดผลลัพธ์ของไจโรสโคปที่สอดคล้องกับค่า RateAxz ที่กล่าวถึงข้างต้น

- เริ่มจากการวางเครื่องในแนวนอน ทั้งเอาต์พุต X และ Y ของมาตรความเร่งจะส่งสัญญาณแรงดันศูนย์-g (เช่น สำหรับบอร์ด Acc_Gyro นี่คือ 1.65V)

- ขั้นต่อไปให้เริ่มหมุนอุปกรณ์รอบแกน Y อีกวิธีหนึ่งคือคุณหมุนอุปกรณ์ในระนาบ XZ เพื่อให้เอาต์พุตของตัวตรวจวัดความเร่ง X และ Z เปลี่ยนไปและเอาต์พุต Y ยังคงที่ - ขณะหมุนอุปกรณ์ด้วยความเร็วคงที่ สังเกตว่าเอาต์พุตของไจโรสโคปเปลี่ยนแปลง เอาต์พุตของไจโรสโคปอื่นๆ ควรคงที่ - เอาต์พุตของไจโรสโคปที่เปลี่ยนแปลงระหว่างการหมุนรอบแกน Y (การหมุนในระนาบ XZ) จะให้ค่าอินพุตสำหรับ AdcGyroXZ จากนั้น เราคำนวณ RateAxz - ขั้นตอนสุดท้ายคือเพื่อให้แน่ใจว่าทิศทางการหมุนสอดคล้องกับแบบจำลองของเรา ในบางกรณีคุณอาจต้องกลับค่า RateAxz เนื่องจากตำแหน่งทางกายภาพของไจโรสโคปที่สัมพันธ์กับมาตรความเร่ง - ทำการทดสอบข้างต้นอีกครั้งโดยหมุนอุปกรณ์ไปรอบๆ แกน Y คราวนี้ตรวจสอบเอาต์พุต X ของมาตรความเร่ง (AdcRx ในรุ่นของเรา) หาก AdcRx โตขึ้น (การหมุน 90 องศาแรกจากตำแหน่งแนวนอน) AdcGyroXZ ก็ควรเติบโตเช่นกัน มิฉะนั้น คุณต้องกลับค่า RateAxz คุณสามารถทำได้โดยแนะนำตัวประกอบสัญญาณในสมการที่ 3 ดังนี้:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity โดยที่ InvertAxz คือ 1 หรือ -1

ทำการทดสอบแบบเดียวกันสำหรับ RateAyz โดยการหมุนอุปกรณ์รอบแกน X และคุณสามารถระบุได้ว่าเอาต์พุตไจโรสโคปใดที่สอดคล้องกับ RateAyz และหากจำเป็นต้องกลับด้าน เมื่อคุณมีค่าสำหรับ InvertAyz แล้ว คุณควรใช้สูตรต่อไปนี้ในการคำนวณ RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / ความไว

หากคุณจะทำการทดสอบเหล่านี้บนบอร์ด Acc_Gyro คุณจะได้ผลลัพธ์ดังต่อไปนี้:

- พินเอาต์พุตสำหรับ RateAxz คือ GX4 และ InvertAxz = -1 - พินเอาต์พุตสำหรับ RateAyz คือ GY4 และ InvertAyz = -1

จากจุดนี้ไป เราจะพิจารณาว่าคุณได้ตั้งค่า IMU ของคุณในลักษณะที่คุณสามารถคำนวณค่าที่ถูกต้องสำหรับ Axr, Ayr, Azr (ตามที่กำหนดไว้ในตอนที่ 1 ความเร่ง) และ RateAxz, RateAyz (ตามที่กำหนดไว้ในตอนที่ 2 Gyroscope). ต่อไป เราจะวิเคราะห์ความสัมพันธ์ระหว่างค่าเหล่านี้ซึ่งเป็นประโยชน์ในการประมาณค่าความเอียงของอุปกรณ์ที่สัมพันธ์กับระนาบพื้นได้แม่นยำยิ่งขึ้น

คุณอาจกำลังถามตัวเองอยู่ ณ จุดนี้ ถ้าแบบจำลองมาตรความเร่งให้มุมเอียงของ Axr, Ayr, Azr แก่เราแล้ว เราจะไปสนใจข้อมูลไจโรสโคปทำไม คำตอบนั้นง่าย: ข้อมูลมาตรความเร่งไม่สามารถเชื่อถือได้ 100% เสมอไป มีสาเหตุหลายประการ โปรดจำไว้ว่ามาตรความเร่งวัดแรงเฉื่อย แรงดังกล่าวอาจเกิดจากแรงโน้มถ่วง (และโดยอุดมคติแล้วโดยแรงโน้มถ่วงเท่านั้น) แต่อาจเกิดจากการเร่ง (การเคลื่อนไหว) ของอุปกรณ์ด้วย แม้ว่ามาตรความเร่งจะอยู่ในสถานะที่ค่อนข้างคงที่ แต่ก็ยังมีความไวต่อการสั่นสะเทือนและเสียงรบกวนจากกลไกโดยทั่วไป นี่คือเหตุผลหลักว่าทำไมระบบ IMU ส่วนใหญ่จึงใช้ไจโรสโคปเพื่อขจัดข้อผิดพลาดของมาตรความเร่ง แต่สิ่งนี้ทำได้อย่างไร? และไจโรสโคปปราศจากเสียงรบกวน ?

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

ในขั้นตอนต่อไป ฉันจะแนะนำอัลกอริธึมที่ได้รับแรงบันดาลใจจากแนวคิดบางอย่างที่ใช้ในตัวกรองคาลมาน อย่างไรก็ตาม การติดตั้งบนอุปกรณ์ฝังตัวนั้นง่ายและง่ายกว่ามาก ก่อนหน้านั้นเรามาดูกันก่อนว่าเราต้องการให้อัลกอริทึมของเราคำนวณอะไร มันคือทิศทางของเวกเตอร์แรงโน้มถ่วง R = [Rx, Ry, Rz] ซึ่งเราสามารถหาค่าอื่นๆ เช่น Axr, Ayr, Azr หรือ cosX, cosY, cosZ ที่จะให้แนวคิดเกี่ยวกับความเอียงของอุปกรณ์ของเรา สัมพันธ์กับระนาบพื้น เราพูดถึงความสัมพันธ์ระหว่างค่าเหล่านี้ในตอนที่ 1 อาจมีคนพูดว่า - เรามีค่าเหล่านี้อยู่แล้วหรือ Rx, Ry, Rz จาก Eq.2 ในส่วนที่ 1? ใช่ แต่จำไว้ว่าค่าเหล่านี้ได้มาจากข้อมูลมาตรความเร่งเท่านั้น ดังนั้น หากคุณต้องการใช้ค่าเหล่านี้โดยตรงในแอปพลิเคชันของคุณ คุณอาจได้รับสัญญาณรบกวนมากกว่าที่แอปพลิเคชันของคุณสามารถทนได้ เพื่อหลีกเลี่ยงความสับสน ให้กำหนดการวัดความเร่งใหม่ดังนี้:

Racc - เป็นเวกเตอร์แรงเฉื่อยที่วัดโดยมาตรความเร่ง ซึ่งประกอบด้วยส่วนประกอบต่อไปนี้ (การฉายภาพบนแกน X, Y, Z):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / ความไว RyAcc = (AdcRy * Vref / 1023 - VzeroG) / ความไว RzAcc = (AdcRz * Vref / 1023 - VzeroG) / ความไว

จนถึงตอนนี้ เรามีชุดค่าที่วัดได้ซึ่งเราหาได้จากค่า ADC ของมาตรความเร่งล้วนๆ เราจะเรียกชุดข้อมูลนี้ว่า "เวกเตอร์" และเราจะใช้สัญกรณ์ต่อไปนี้

Racc = [RxAcc, RyAcc, RzAcc]

เนื่องจากส่วนประกอบเหล่านี้ของ Racc สามารถรับได้จากข้อมูลมาตรความเร่ง เราจึงพิจารณาได้ว่ามันเป็นอินพุตสำหรับอัลกอริทึมของเรา

โปรดทราบว่าเนื่องจาก Racc วัดแรงโน้มถ่วง คุณจะถูกต้องถ้าคุณถือว่าความยาวของเวกเตอร์นี้ที่กำหนดไว้ดังต่อไปนี้เท่ากับหรือใกล้เคียงกับ 1g

|แรค| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2), อย่างไรก็ตาม เพื่อให้มั่นใจว่าควรอัปเดตเวกเตอร์นี้ดังนี้:

Racc(ทำให้เป็นมาตรฐาน) = [RxAcc/|Racc|, RyAcc/|Racc|, RzAcc/|Racc|].

เพื่อให้แน่ใจว่าความยาวของเวกเตอร์ Racc ที่ทำให้เป็นมาตรฐานของคุณจะเป็น 1 เสมอ

ต่อไปเราจะแนะนำเวกเตอร์ใหม่และเราจะเรียกมันว่า

ส่วนที่เหลือ = [RxEst, RyEst, RzEst]

นี่จะเป็นผลลัพธ์ของอัลกอริธึมของเรา ซึ่งเป็นค่าที่แก้ไขโดยอิงจากข้อมูลไจโรสโคปและอิงตามข้อมูลโดยประมาณในอดีต

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

มาดูกันว่าเราจะทำให้มันทำงานได้อย่างไร

เราจะเริ่มลำดับโดยวางใจมาตรความเร่งของเราและกำหนด:

พักผ่อน(0) = แรค(0)

โปรดจำไว้ว่า Rest และ Racc เป็นเวกเตอร์ ดังนั้นสมการข้างต้นจึงเป็นวิธีง่ายๆ ในการเขียนสมการ 3 ชุด และหลีกเลี่ยงการซ้ำซ้อน:

RxEst(0) = RxAcc(0) RyEst(0) = RyAcc(0) RzEst(0) = RzAcc(0)

ต่อไป เราจะทำการวัดอย่างสม่ำเสมอในช่วงเวลาที่เท่ากันของ T วินาที และเราจะได้รับการวัดใหม่ที่เราจะกำหนดเป็น Racc(1), Racc(2), Racc(3) และอื่นๆ นอกจากนี้เรายังจะออกค่าประมาณใหม่ในแต่ละช่วงเวลา Rest(1), Rest(2), Rest(3) และอื่นๆ

สมมติว่าเราอยู่ที่ขั้นตอนที่ n เรามีค่าที่รู้จักสองชุดที่เราต้องการใช้:

Rest(n-1) - การประมาณการครั้งก่อนของเรา โดย Rest(0) = Racc(0) Racc(n) - การวัดความเร่งของเราในปัจจุบัน

ก่อนที่เราจะคำนวณ Rest(n) เรามาแนะนำค่าที่วัดได้ใหม่ที่เราหาได้จากไจโรสโคปและการประมาณการครั้งก่อน

เราจะเรียกมันว่า Rgyro และยังเป็นเวกเตอร์ที่ประกอบด้วย 3 องค์ประกอบ:

Rgyro = [RxGyro, RyGyro, RzGyro]

เราจะคำนวณเวกเตอร์นี้ทีละองค์ประกอบ เราจะเริ่มด้วย RxGyro

เริ่มต้นด้วยการสังเกตความสัมพันธ์ต่อไปนี้ในแบบจำลองไจโรสโคป จากสามเหลี่ยมมุมฉากที่เกิดจาก Rz และ Rxz เราสามารถสรุปได้ว่า:

ตาล(Axz) = Rx/Rz => Axz = atan2(Rx, Rz)

Atan2 อาจเป็นฟังก์ชันที่คุณไม่เคยใช้มาก่อน ซึ่งคล้ายกับ atan เว้นแต่จะส่งกลับค่าในช่วง (-PI, PI) ซึ่งต่างจาก (-PI/2, PI/2) ที่ส่งกลับโดย atan และต้องใช้ 2 อาร์กิวเมนต์แทนที่จะเป็นหนึ่ง ช่วยให้เราสามารถแปลงค่า Rx, Rz สองค่าเป็นมุมได้เต็มช่วง 360 องศา (-PI ถึง PI) คุณสามารถอ่านเพิ่มเติมเกี่ยวกับ atan2 ได้ที่นี่

ดังนั้นเมื่อรู้ RxEst(n-1) และ RzEst(n-1) เราจะพบว่า:

Axz(n-1) = atan2(RxEst(n-1), RzEst(n-1))

โปรดจำไว้ว่าไจโรสโคปวัดอัตราการเปลี่ยนแปลงของมุม Axz เราสามารถประมาณค่ามุมใหม่ Axz(n) ได้ดังนี้:

Axz(n) = Axz(n-1) + RateAxz(n) * T

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

RateAxzAvg = (RateAxz(n) + RateAxz(n-1)) / 2 Axz(n) = Axz(n-1) + RateAxzAvg * T

วิธีเดียวกับที่เราสามารถหาได้:

Ayz(n) = Ayz(n-1) + อัตราAyz(n) * T

ตกลง ตอนนี้เรามี Axz(n) และ Ayz(n) เราจะไปจากที่นี่เพื่อหัก RxGyro/RyGyro ได้ที่ไหน? จากสมการ 1 เราสามารถเขียนความยาวของเวกเตอร์ Rgyro ได้ดังนี้:

|Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)

เนื่องจากเราปรับเวกเตอร์ Racc ให้เป็นมาตรฐาน เราอาจสันนิษฐานได้ว่าความยาวเท่ากับ 1 และไม่มีการเปลี่ยนแปลงหลังการหมุน ดังนั้นจึงค่อนข้างปลอดภัยที่จะเขียน:

|Rgyro| = 1

ลองใช้สัญกรณ์ที่สั้นกว่านี้ชั่วคราวสำหรับการคำนวณด้านล่าง:

x =RxGyro, y=RyGyro, z=RzGyro

ใช้ความสัมพันธ์ข้างต้นเราสามารถเขียน:

x = x / 1 = x / SQRT(x^2+y^2+z^2)

มาหารตัวเศษและตัวส่วนของเศษส่วนด้วย SQRT(x^2 + z^2)

x = (x / SQRT(x^2 + z^2)) / SQRT((x^2 + y^2 + z^2) / (x^2 + z^2))

โปรดทราบว่า x / SQRT(x^2 + z^2) = sin(Axz) ดังนั้น:

x = บาป(Axz) / SQRT (1 + y^2 / (x^2 + z^2))

ตอนนี้คูณตัวเศษและตัวส่วนของเศษส่วนใน SQRT ด้วย z^2

x = บาป(Axz) / SQRT (1 + y^2 * z ^2 / (z^2 * (x^2 + z^2)))

โปรดทราบว่า z / SQRT(x^2 + z^2) = cos(Axz) และ y / z = tan(Ayz) ดังนั้นสุดท้าย:

x = บาป(Axz) / SQRT (1 + cos(Axz)^2 * แทน (Ayz)^2)

กลับไปที่สัญกรณ์ของเราเราได้รับ:

RxGyro = บาป(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2)

แบบเดียวกับที่เราพบว่า

RyGyro = บาป(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2)

ในที่สุดเราก็สามารถพบ:

RzGyro = เครื่องหมาย(RzGyro)*SQRT(1 - RxGyro^2 - RyGyro^2)

โดยที่ Sign(RzGyro) = 1 เมื่อ RzGyro>=0 และ Sign(RzGyro) = -1 เมื่อ RzGyro<0

วิธีง่ายๆ ในการประมาณค่านี้คือ:

เครื่องหมาย(RzGyro) = เครื่องหมาย(RzEst(n-1))

ในทางปฏิบัติ โปรดใช้ความระมัดระวังเมื่อ RzEst(n-1) เข้าใกล้ 0 คุณอาจข้ามเฟสไจโรทั้งหมดในกรณีนี้และกำหนด: Rgyro = Rest(n-1) Rz ถูกใช้เป็นข้อมูลอ้างอิงสำหรับการคำนวณมุม Axz และ Ayz และเมื่อค่าใกล้ถึง 0 ค่าอาจล้นออกมาและทำให้เกิดผลลัพธ์ที่ไม่ดี คุณจะอยู่ในโดเมนของตัวเลขทศนิยมขนาดใหญ่ซึ่งการใช้งานฟังก์ชัน tan() / atan() อาจขาดความแม่นยำ

มาสรุปสิ่งที่เรามีตอนนี้ เราอยู่ในขั้นตอนที่ n ของอัลกอริทึมของเรา และเราได้คำนวณค่าต่อไปนี้:

Racc - ค่าที่อ่านได้จากเครื่องวัดความเร่ง Rgyro ของเรา - ได้จาก Rest(n-1) และค่าที่อ่านได้จากไจโรสโคปปัจจุบัน

เราใช้ค่าใดในการคำนวณค่าประมาณการที่ปรับปรุงแล้ว Rest(n) ? คุณอาจเดาได้ว่าเราจะใช้ทั้งสองอย่าง เราจะใช้ค่าเฉลี่ยถ่วงน้ำหนัก เพื่อ:

ส่วนที่เหลือ (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

เราสามารถลดความซับซ้อนของสูตรนี้ได้โดยหารทั้งตัวเศษและตัวส่วนของเศษส่วนด้วย w1

ส่วนที่เหลือ(n) = (Racc * w1/w1 + Rgyro * w2/w1) / (w1/w1 + w2/w1)

และหลังจากแทนที่ w2/w1 = wGyro เราได้รับ:

ส่วนที่เหลือ (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

ในฟอรัม wGyro ด้านบนบอกเราว่าเราไว้วางใจไจโรมากแค่ไหนเมื่อเทียบกับมาตรความเร่งของเรา ค่านี้สามารถเลือกทดลองได้ โดยปกติค่าระหว่าง 5..20 จะทำให้เกิดผลลัพธ์ที่ดี

ความแตกต่างที่สำคัญของอัลกอริธึมนี้จากตัวกรองคาลมานก็คือ น้ำหนักนี้ค่อนข้างคงที่ ในขณะที่ตัวกรองคาลมาน ตุ้มน้ำหนักจะได้รับการอัปเดตอย่างถาวรตามเสียงที่วัดได้ของการอ่านค่าความเร่ง ตัวกรองคาลมานมุ่งเน้นไปที่การให้ผลลัพธ์ทางทฤษฎีที่ "ดีที่สุด" ในขณะที่อัลกอริธึมนี้สามารถให้ผลลัพธ์ที่ "ดีพอ" สำหรับการใช้งานจริงของคุณ คุณสามารถใช้อัลกอริทึมที่ปรับ wGyro โดยขึ้นอยู่กับปัจจัยรบกวนบางอย่างที่คุณวัด แต่ค่าคงที่จะทำงานได้ดีสำหรับแอปพลิเคชันส่วนใหญ่

เหลืออีกเพียงขั้นตอนเดียวในการรับค่าประมาณที่อัปเดตแล้ว:

RxEst(n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro) RyEst(n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro) RzEst(n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)

ทีนี้มาทำให้เวกเตอร์นี้เป็นมาตรฐานอีกครั้ง:

R = SQRT(RxEst(n) ^2 + RyEst(n)^2 + RzEst(n)^2)

RxEst(n) = RxEst(n)/R RyEst(n) = RyEst(n)/R RzEst(n) = RzEst(n)/R

และเราพร้อมที่จะวนซ้ำอีกครั้ง

คู่มือนี้เดิมปรากฏบน starlino.com ฉันได้แก้ไขเล็กน้อยและโพสต์ใหม่โดยได้รับอนุญาต ขอบคุณสตาร์ลิโน!