מד תאוצה וג'ירו הדרכה: 3 שלבים

2024 מְחַבֵּר: John Day | [email protected]. שונה לאחרונה: 2024-01-30 09:18

מבוא

מדריך זה מיועד לכל מי שמעוניין להשתמש במדי תאוצה וג'ירוסקופים וכן במכשירי IMU משולבים (יחידת מדידת אינרציה) בפרויקטים האלקטרוניים שלהם.

נכסה:

• מה מודד מד תאוצה?
• מה מודד ג'ירוסקופ (aka ג'ירו)?
• כיצד להמיר קריאות אנלוגיות לדיגיטליות (ADC) שאתה מקבל מהחיישן ליחידות פיזיות (אלה יהיו g עבור מד תאוצה, deg/s עבור גירוסקופ)
• כיצד לשלב קריאות מד תאוצה וג'ירוסקופ על מנת לקבל מידע מדויק על נטיית המכשיר ביחס למישור הקרקע

לאורך המאמר אנסה לצמצם את המתמטיקה למינימום. אם אתה יודע מה זה סינוס/קוסינוס/משיק, אתה אמור להיות מסוגל להבין ולהשתמש ברעיונות אלה בפרויקט שלך, לא משנה באיזו פלטפורמה אתה משתמש: Arduino, מדחף, חותמת בסיסית, שבבי Atmel, מיקרו -שבב PIC וכו '.

יש שם אנשים שמאמינים שאתה צריך מתמטיקה מורכבת על מנת להשתמש ביחידת IMU (מסנני FIR או IIR מורכבים כגון מסנני קלמן, מסנני פארקס-מק'קלן וכו '). אתה יכול לחקור את כל אלה ולהשיג תוצאות נפלאות אך מורכבות. הדרך שלי להסביר דברים דורשת מתמטיקה בסיסית בלבד. אני מאמין גדול בפשטות. אני חושב שמערכת פשוטה יותר לשליטה ולניטור, מלבד התקנים משובצים רבים אין להם את הכוח והמשאבים ליישם אלגוריתמים מורכבים הדורשים חישובי מטריצה.

אשתמש כדוגמה ביחידת IMU חדשה, Acc_Gyro Accelerometer + Gyro IMU. נשתמש בפרמטרים של מכשיר זה בדוגמאות שלנו להלן. יחידה זו היא מכשיר טוב להתחלה מכיוון שהיא מורכבת משני מכשירים:

- LIS331AL (גיליון נתונים) - מד תאוצה 2G טריאקסיאלי - LPR550AL (גליון נתונים) - גובה וציר גליל דו -ציריים, גירוסקופ של 500 מעלות לשנייה

יחד הם מייצגים יחידת מדידה אינרציאלית של חירות בת 5 דרגות. עכשיו זה שם מפואר! עם זאת, מאחורי השם המפואר עומד מכשיר שילוב שימושי מאוד עליו נסקור ונסביר בפירוט במדריך זה.

שלב 1: מד התאוצה

כדי להבין את היחידה הזו נתחיל במד התאוצה. כשחושבים על מדי תאוצה זה לעתים קרובות שימושי לדמיין קופסה בצורת קובייה עם כדור בתוכה. אתה יכול לדמיין משהו אחר כמו עוגיה או סופגנייה, אבל אני מדמיין כדור:

אם ניקח את הקופסה הזו במקום ללא שדות כבידה או לצורך העניין ללא שדות אחרים שעלולים להשפיע על מיקום הכדור - הכדור פשוט יצוף באמצע הקופסה. אתה יכול לדמיין שהתיבה נמצאת בחלל החיצון רחוק-רחוק מכל גופים קוסמיים, או אם קשה למצוא מקום כזה, דמיין לפחות כלי שיט בחלל המסתובבים סביב הפלנטה שבה הכל במצב חסר משקל. מהתמונה למעלה אתה יכול לראות שאנחנו מקצים לכל ציר זוג קירות (הסרנו את הקיר Y+ כדי שנוכל להביט בתוך הקופסה). תארו לעצמכם שכל קיר רגיש ללחץ. אם נזיז לפתע את הקופסה שמאלה (אנו מאיצים אותה בתאוצה 1g = 9.8m/s^2), הכדור יפגע בקיר X-. לאחר מכן אנו מודדים את כוח הלחץ שהכדור מפעיל על הקיר ומפיק ערך של -1 גרם על ציר ה- X.

שימו לב שמד התאוצה אכן יזהה כוח שמופנה בכיוון ההפוך מכווץ התאוצה. כוח זה נקרא לעתים קרובות אינרציה כוח או כוח פיקטיבי. דבר אחד שכדאי ללמוד מכך הוא שמד תאוצה מודד תאוצה בעקיפין באמצעות כוח המופעל על אחד מקירותיו (על פי המודל שלנו, זה עשוי להיות קפיץ או משהו אחר במד תאוצה בחיים האמיתיים). כוח זה יכול להיגרם מהתאוצה, אך כפי שנראה בדוגמה הבאה הוא לא תמיד נגרם על ידי האצה.

אם ניקח את המודל שלנו ונניח אותו על כדור הארץ הכדור ייפול על קיר Z ויחיל כוח של 1 גרם על הקיר התחתון, כפי שמוצג בתמונה למטה:

במקרה זה התיבה לא זזה אך עדיין אנו מקבלים קריאה של -1 גרם על ציר Z. הלחץ שהכדור הפעיל על הקיר נגרם מכוח כבידה. בתיאוריה זה יכול להיות סוג אחר של כוח - למשל, אם אתה מתאר לעצמך שהכדור שלנו מתכתי, הצבת מגנט ליד הקופסה עלולה להזיז את הכדור כך שהוא פוגע בקיר אחר. זה נאמר רק כדי להוכיח שבמהות אמצעי מד התאוצה לא מכריחים תאוצה. זה פשוט קורה שהתאוצה גורמת לכוח אינרציאלי שנלכד על ידי מנגנון זיהוי הכוח של מד התאוצה.

אמנם מודל זה אינו בדיוק אופן בניית חיישן MEMS אך לעיתים קרובות הוא שימושי בפתרון בעיות הקשורות למד תאוצה. יש למעשה חיישנים דומים שיש להם כדורים מתכתיים בפנים, הם נקראים מתגי הטיה, אולם הם פרימיטיביים יותר ובדרך כלל הם יכולים לדעת רק אם המכשיר נוטה בטווח מסוים או לא, לא מידת הנטייה.

עד כה ניתחנו את תפוקת מד התאוצה על ציר יחיד וזה כל מה שתקבל עם מדי תאוצה בציר יחיד. הערך האמיתי של מד תאוצה טריאקסיאלי נובע מהעובדה שהם יכולים לזהות כוחות אינרציה על כל שלושת הצירים. נחזור לדגם התיבה שלנו, ובוא נסובב את התיבה 45 מעלות ימינה. הכדור יגע כעת ב -2 קירות: Z- ו- X- כפי שמוצג בתמונה למטה:

הערכים של 0.71 אינם שרירותיים, הם למעשה קירוב ל- SQRT (1/2). זה יתבהר ככל שנציג את הדגם הבא שלנו עבור מד התאוצה.

בדגם הקודם תיקנו את כוח הכבידה וסובבנו את התיבה הדמיונית שלנו. בשתי הדוגמאות האחרונות ניתחנו את הפלט בשתי עמדות תיבה שונות, בעוד וקטור הכוח נשאר קבוע. אמנם זה היה שימושי להבנת האופן שבו מד התאוצה מתקיים עם כוחות חיצוניים, אך מעשי יותר לבצע חישובים אם נתקן את מערכת הקואורדינטות לצירים של מד התאוצה ונדמיין כי וקטור הכוח מסתובב סביבנו.

אנא תסתכל על הדגם למעלה, שימרתי את צבעי הצירים כך שתוכל לבצע מעבר נפשי מהדגם הקודם לדגם החדש. רק תארו לעצמכם שכל ציר בדגם החדש הוא בניצב לפנים המתאימות של התיבה בדגם הקודם. הווקטור R הוא וקטור הכוח שמד התאוצה מודד (זה יכול להיות כוח הכבידה או כוח האינרציה מהדוגמאות לעיל או שילוב של שניהם). Rx, Ry, Rz הם הקרנה של וקטור R בצירים X, Y, Z. אנא שימו לב לקשר הבא:

R^2 = Rx^2 + Ry^2 + Rz^2 (שוויון 1)

שזה בעצם המקבילה למשפט הפיתגוראי בתלת מימד.

זכור שקצת קודם אמרתי לך שהערכים של 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.

בואו נמשיך על ידי בחינת דוגמה פשוטה, נניח שמודול ה- ADC שלנו בנפח 10 ביט נתן לנו את הערכים הבאים לשלושת ערוצי מד התאוצה (צירים):

AdcRx = 586 AdcRy = 630 AdcRz = 561

לכל מודול ADC יהיה מתח ייחוס, נניח בדוגמה שלנו שהוא 3.3V. כדי להמיר ערך ADC של 10 ביט למתח אנו משתמשים בנוסחה הבאה:

VoltsRx = AdcRx * Vref / 1023

הערה מהירה כאן: כי עבור 8 סיביות ADC המחיצה האחרונה תהיה 255 = 2 ^ 8 -1, ועבור 12 ביט ADC המחיצה האחרונה תהיה 4095 = 2 ^ 12 -1.

החלת נוסחה זו על כל שלושת הערוצים שאנו מקבלים:

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

כעת יש לנו את מדידות התאוצה שלנו בוולט, זה עדיין לא ב- 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) / רגישות (שווי 2) Ry = (AdcRy * Vref / 1023 - VzeroG) / רגישות Rz = (AdcRz * Vref / 1023 - VzeroG) / רגישות

כעת יש לנו את כל 3 המרכיבים המגדירים את וקטור כוח האינרציה שלנו, אם המכשיר אינו כפוף לכוחות אחרים מלבד הכבידה, אנו יכולים להניח שזה הכיוון של וקטור כוח הכבידה שלנו. אם אתה רוצה לחשב את נטיית המכשיר ביחס לקרקע אתה יכול לחשב את הזווית בין וקטור זה לציר Z. אם אתה מתעניין גם בכיוון הנטייה לכל ציר אתה יכול לחלק תוצאה זו לשני מרכיבים: נטייה על ציר 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 () (הפונקציה 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

שלישיה זו נקראת לעתים קרובות כיוון קוסינוס, והיא בעצם מייצגת את וקטור היחידה (וקטור באורך 1) בעל כיוון זהה לזה של וקטור ה- R שלנו. תוכל לוודא בקלות כי:

SQRT (cosX^2 + נעים^2 + cosZ^2) = 1

זהו נכס נחמד מכיוון שהוא פוטר אותנו מלנטר את המודולוס (האורך) של וקטור R. פעמים רבות אם אנו מעוניינים רק בכיוון של וקטור האינרציה שלנו, הגיוני לנרמל את המודולוס שלו כדי לפשט חישובים אחרים.

שלב 2: ג'ירוסקופ

אנחנו לא הולכים להציג מודל תיבה שווה לג'ירוסקופ כמו שעשינו עבור מד תאוצה, במקום זאת אנחנו נקפוץ ישר לדגם השני של מד התאוצה ונראה מה המודד הג'ירוסקופ לפי מודל זה.

כל ערוץ ג'ירוסקופ מודד את הסיבוב סביב אחד הצירים. למשל, גירוסקופ בעל 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

עכשיו אנחנו מתקרבים למה שהג'ירוסקופ מודד. ג'ירוסקופ מודד את קצב השינויים בזוויות שהוגדרו לעיל. במילים אחרות הוא יפיק ערך שקשור באופן לינארי לקצב השינוי של זוויות אלה. כדי להסביר זאת נניח שמדדנו את זווית הסיבוב סביב ציר Y (זו תהיה זווית Axz) בזמן t0, ואנו מגדירים אותה כ- Axz0, לאחר מכן מדדנו את הזווית הזו בזמן מאוחר יותר t1 וזה היה Axz1. שיעור השינוי יחושב כדלקמן:

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

אם נבטא את Axz במעלות, והזמן בשניות, אז ערך זה יתבטא ב- deg/s. זה מה שגירוסקופ מודד.

בפועל ג'ירוסקופ (אלא אם כן מדובר בג'ירוסקופ דיגיטלי מיוחד) רק לעתים רחוקות ייתן לך ערך המתבטא ב- deg/s. אותו דבר כמו עבור מד תאוצה תקבל ערך ADC אותו תצטרך להמיר ל- deg/s באמצעות נוסחה הדומה ל- Eq. 2 שהגדרנו עבור מד תאוצה. הבה נציג את נוסחת ההמרה של ADC ל- deg/s לג'ירוסקופ (אנו מניחים שאנו משתמשים במודול ADC של 10bit, עבור 8bit ADC מחליף 1023 ב- 255, ב- 12bit ADC מחליף 1023 ב- 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / רגישות שווי 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 יהיה תפוקת הג'ירוסקופ תגדל, אם תגדיל את מהירות הסיבוב ב- deg/s. הרגישות של לוח 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 של מד התאוצה יפיקו את מתח האפס-גרם (למשל ללוח Acc_Gyro זה 1.65V)

- התחל בשלב הבא לסובב את המכשיר סביב ציר Y, דרך נוספת לומר זאת היא שאתה מסובב את המכשיר במישור XZ, כך שיציאות מד התאוצה X ו- Z משתנות ופלט Y נשאר קבוע. - בזמן סיבוב המכשיר במהירות הערה קבועה אשר פלט הג'ירוסקופ משתנה, יציאות הג'ירוסקופ האחרות צריכות להישאר קבועות - פלט הג'ירוסקופ שהשתנה במהלך הסיבוב סביב ציר Y (סיבוב במישור XZ) יספק את ערך הקלט עבור AdcGyroXZ, שממנו אנו מחשבים את RateAxz - השלב האחרון הוא להבטיח כיוון הסיבוב תואם את המודל שלנו, במקרים מסוימים ייתכן שיהיה עליך להפוך את ערך RateAxz בשל המיקום הפיזי של הג'ירוסקופ ביחס למד התאוצה - בצע שוב את הבדיקה שלעיל, וסובב את המכשיר מסביב ציר Y, הפעם עקוב אחר תפוקת ה- X של מד התאוצה (AdcRx במודל שלנו). אם AdcRx צומח (90 מעלות הסיבוב הראשונות ממיקום אופקי), אז AdcGyroXZ צריך גם לצמוח. אחרת עליך להפוך את RateAxz, תוכל להשיג זאת על ידי הצגת גורם סימן ב- Eq.3, כדלקמן:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / רגישות, כאשר 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. גירוסקופ). בשלב הבא ננתח את היחסים בין ערכים אלה המתבררים כשימושיים בהשגת אומדן מדויק יותר של נטיית המכשיר ביחס למישור הקרקע.

ייתכן שאתה שואל את עצמך בשלב זה, אם דגם מד התאוצה כבר נתן לנו זוויות נטייה של אקסר, אייר, אזר מדוע נרצה להתעסק בנתוני הג'ירוסקופ? התשובה פשוטה: לא תמיד ניתן לסמוך על נתוני מד תאוצה במאה אחוז. ישנן מספר סיבות, זכור כי מד התאוצה מודד כוח אינרציה, כוח כזה יכול להיגרם כתוצאה מכוח הכבידה (ובאופן אידיאלי רק על ידי כבידה), אך הוא עשוי להיגרם גם על ידי האצה (תנועה) של המכשיר. כתוצאה מכך גם אם מד התאוצה נמצא במצב יציב יחסית, הוא עדיין רגיש מאוד לרטט ולרעש מכני באופן כללי. זו הסיבה העיקרית לכך שרוב מערכות ה- IMU משתמשות בג'ירוסקופ כדי להחליק כל שגיאת מד תאוצה. אך כיצד הדבר מתבצע? והאם הג'ירוסקופ נקי מרעש?

הג'ירוסקופ אינו נקי מרעש אולם מכיוון שהוא מודד סיבוב הוא פחות רגיש לתנועות מכאניות לינאריות, סוג הרעש שמד תאוצה סובל ממנו, אולם לג'ירוסקופים יש סוגים אחרים של בעיות כמו למשל סחף (לא חוזר לערך בשיעור אפס כאשר הסיבוב מפסיק). אף על פי כן, על ידי ממוצע נתונים המגיעים ממד תאוצה וג'ירוסקופ אנו יכולים להשיג אומדן טוב יותר יחסית של נטיית המכשיר הנוכחית מכפי שהיינו משיגים באמצעות נתוני מד התאוצה בלבד.

בשלבים הבאים אציג אלגוריתם שקיבל השראה מכמה רעיונות המשמשים במסנן קלמן, אולם הוא הרבה יותר פשוט וקל ליישום במכשירים משובצים. לפני כן בואו נראה קודם מה אנו רוצים שהאלגוריתם שלנו יחשב. ובכן, זהו כיוון כוח הכבידה R = [Rx, Ry, Rz] שממנו נוכל להפיק ערכים אחרים כמו Axr, Ayr, Azr או cosX, cosy, cosZ שייתן לנו מושג לגבי נטיית המכשיר שלנו ביחס למישור הקרקע, אנו דנים ביחס בין ערכים אלה בחלק 1. אפשר לומר - האם אין לנו כבר ערכים אלה Rx, Ry, Rz משוואה 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 מנתוני מד תאוצה, אנו יכולים לראות בכך כקלט לאלגוריתם שלנו.

שים לב שמכיוון שראק מודד את כוח הכבידה אתה תהיה צודק אם אתה מניח שאורך הווקטור הזה המוגדר כדלקמן שווה או קרוב ל -1 גרם.

| ראק | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), עם זאת כדי להיות בטוח שזה הגיוני לעדכן את הווקטור הזה כדלקמן:

Racc (מנורמל) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |].

זה יבטיח שאורך וקטור ה- Racc המנורמל שלך יהיה תמיד 1.

בשלב הבא נציג וקטור חדש ונקרא לזה

מנוחה = [RxEst, RyEst, RzEst]

זו תהיה הפלט של האלגוריתם שלנו, אלה ערכים מתוקנים המבוססים על נתוני גירוסקופ ומבוססים על נתוני הערכה קודמים.

הנה מה האלגוריתם שלנו יעשה: - מד תאוצה אומר לנו: "אתה נמצא כעת במיקום Racc" - אנו אומרים "תודה, אבל תן לי לבדוק", - ואז תקן את המידע הזה עם נתוני גירוסקופ כמו גם עם נתוני מנוחה בעבר ו אנו מפיקים מנוחה וקטורית משוערת חדשה. - אנו רואים במנוחה את "ההימור הטוב ביותר" שלנו לגבי המיקום הנוכחי של המכשיר.

בואו נראה איך נוכל לגרום לזה לעבוד.

נתחיל את הרצף שלנו על ידי אמון במד התאוצה שלנו והקצאת:

מנוחה (0) = Racc (0)

אגב זכור מנוחה וראק הם וקטורים, ולכן המשוואה לעיל היא רק דרך פשוטה לכתוב 3 קבוצות משוואות ולהימנע מחזרה:

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

בשלב הבא נבצע מדידות סדירות במרווחי זמן שווים של T שניות, ונקבל מדידות חדשות שנקבע כ- Racc (1), Racc (2), Racc (3) וכן הלאה. כמו כן, ננפיק הערכות חדשות בכל מרווחי זמן מנוחה (1), מנוחה (2), מנוחה (3) וכן הלאה.

נניח שאנחנו בשלב n. יש לנו שתי קבוצות ערכים ידועות בהן נרצה להשתמש:

מנוחה (n -1) - ההערכה הקודמת שלנו, עם מנוחה (0) = Racc (0) Racc (n) - מדידת מד התאוצה הנוכחית שלנו

לפני שנוכל לחשב את המנוחה (n), בואו להציג ערך מדוד חדש, שנוכל להשיג מהג'ירוסקופ שלנו ומהערכה קודמת.

נקרא לזה Rgyro, וזה גם וקטור המורכב משלושה רכיבים:

Rgyro = [RxGyro, RyGyro, RzGyro]

נחשב את הווקטור הזה רכיב אחד בכל פעם. נתחיל ב- RxGyro.

נתחיל בהתבוננות ביחס הבא במודל הג'ירוסקופ שלנו, ממשולש הזווית הימנית שנוצרו על ידי Rz ו- Rxz נוכל להסיק זאת:

שיזוף (Axz) = Rx/Rz => Axz = atan2 (Rx, Rz)

Atan2 עשויה להיות פונקציה שמעולם לא השתמשת בה בעבר, היא דומה לאטאן, אלא שהוא מחזיר ערכים בטווח של (-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) + RateAyz (n) * T

אוקי אז עכשיו יש לנו Axz (n) ו- Ayz (n). לאן נלך מכאן כדי לנכות את RxGyro/RyGyro? מתוך Eq. 1 נוכל לכתוב את אורך הווקטור Rgyro כדלקמן:

| רג'ירו | = SQRT (RxGyro^2 + RyGyro^2 + RzGyro^2)

גם מכיוון שנירמלנו את וקטור ה- Racc שלנו, אנו עשויים להניח שאורכו הוא 1 והוא לא השתנה לאחר הסיבוב, ולכן בטוח יחסית לכתוב:

| רג'ירו | = 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 = sin (Axz) / SQRT (1 + y^2 / (x^2 + z^2))

כעת הכפל את המונה ואת המכנה של השבר בתוך SQRT ב- z^2

x = sin (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 = sin (Axz) / SQRT (1 + cos (Axz)^2 * tan (Ayz)^2)

אם נחזור לסימון שלנו נקבל:

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

באותה דרך שאנחנו מוצאים את זה

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

כעת, סוף סוף אנו יכולים למצוא:

RzGyro = סימן (RzGyro)*SQRT (1 - RxGyro^2 - RyGyro^2).

איפה סימן (RzGyro) = 1 כאשר RzGyro> = 0, וסימן (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 - המתקבלות מהקריאות מנוחה (n -1) וגירוסקופ הנוכחי

באילו ערכים אנו משתמשים כדי לחשב את ההערכה המעודכנת מנוחה (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, ערכתי כמה עריכות קלות ופרסמתי אותו מחדש באישור. תודה סטארלינו!