ניווט אינרציאלי מבוסס על Unscented Kalman Filter

הטכניון – מכון טכנולוגי לישראל הפקולטה להנדסת חשמל המעבדה לבקרה ורובוטיקה ניווט אינרציאלי מבוסס על Unscented KalmanFilter איליה אוסדצ'י ודים אוחרימנקו בהנחיית ד''ר גבי דוידוב

נושאי הדיון • מוטיבציה • מטרות הפרויקט • ניווט אינרציאלי • Unscented Kalman Filter • מימוש "בסיסי" • הורדת סיבוכיות • scaling factor • סיכום ומסקנות

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

מטרות הפרויקט • בנייה של מערכת ניווט אינרציאלי • טיפול ברעשים בעזרת Unscented Kalman Filter • השוואה בין מימושים שונים • הורדת סיבוכיות • התחשבות בשגיאת scaling factor

ניווט אינרציאלי - מבוא • ניווט אינרציאלי הינו שיטת ניווט שמחשבת מצב נוכחי על סמך מצב קודם ונתוני חיישנים • מצב: מיקום, זווית, מהירות • חיישנים: תאוצה, מהירות זוויתית • עבודה עם שתי מערכות צירים • מערכת הגוף, מערכת הארץ • מעבר בין מערכות בעזרתמטריצת סיבוב שתלויה בזווית • הזווית מכילה שלושה מרכיבים:roll, pitch, yaw (φ, θ, ψ)

ניווט אינרציאלי - טרנספורמציות • מטריצות טרנספורמציה בין מערכות צירים תלויות בזוויות roll, pitch, yaw • מטריצת טרנספורמציה של תאוצה או מהירות: • מטריצת טרנספורמציה של מהירות זוויתית:

ניווט אינרציאלי - שגיאות • בעיה עיקרית של ניווט אינרציאלי הינה שגיאות מדידה • מימוש ישיר מבצע אינטגרציה על אות רועש שמביאה להתבדרות • סוגי שגיאות: • רעש לבן • היסט (bias) • קנה מידה (scaling factor)

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

UKF - המשך • אלגוריתם בסיסי: • חיזוי של מצב הבא (xk|k-1) על סמך מצב קודם (xk-1|k-1) וחיישנים (u) • חישוב פלט צפוי (yk|k-1) • השוואה לפלט נמדד (yk) כלומר מדידת אודומטר • תיקון מצב • המסנן נותן משקלים למשתני מצב ומדידות בהתאם לרמת בטחון • שומר ומעדכן מטריצתקווריאנס של וקטור מצב • יודע מטריצות קווריאנס של רעש המדידות • מעדכן מצב וקווריאנס המצב עם התחשבות בקווריאנסים

Unscented transformation • דרוש להפעיל טרנספורמציה לא ליניארית על וקטור אקראי • פיתרון אפשרי: Unscented transformation • בוחרים נקודות סביב תוחלתבהתאם לקווריאנס(“sigma-points”) • מפעילים טרנספורמציה עלכל נקודה • מחשבים תוחלת וקווריאנסעל סמך נקודות שהתקבלולפי משקלים (Ws, Wc)

מימוש "בסיסי" • במימוש הבסיסי שלנו נחלק את הבעיה לחלק ליניארי וחלק לא-ליניארי (Rao-Blackwellised UKF) • חיזוי מצב (f()) לא-ליניארי וחיזוי פלט (h()) ליניארי • לכן שומרים בווקטור מצב מהירות במערכת צירים מסובבת • מתחשבים בשגיאת היסט של חיישנים • שומרים היסט משוערך בווקטור מצב • עדכון נעשה יחד עם תיקון המצב • וקטור המצב: [מיקום(3), מהירות(3), זוויות(3), היסטתאוצה(3), היסטגירוסקופים(3), מרחק] • סה''כ 16 רכיבים

מימוש "בסיסי" – דיאגראמת בלוקים

מימוש "בסיסי" – פרטים • יצירת sigma points • יוצרים מצב מורחב (augmented) מווקטור מצב ונתוני חיישנים (סה''כL=22 רכיבים): • מחשבים sigma points: כאשר פעולת שורש על מטריצה הינה Cholesky decomposition

מימוש "בסיסי" – פרטים • טרנספורמציה של sigma points • על כל נקודה χik-1|k-1 מפעילים טרנספורמציה לא ליניארית ומקבלים נקודה χik|k-1

מימוש "בסיסי" – פרטים • מתוך sigma points החדשים (χik|k-1) מקבלים חיזוי המצב הבא: • מתוך חיזוי מצב מחשבים חיזוי מדידה (y^k|k-1)וקווריאנסים בצורה ליניארית: • תיקון מצב:

גרף:מסלול מחושב לפי קלט אמיתי • אורך מסלול: • 7.4 ק''מ • רעש משוערך (סטיית תקן): • מדי תאוצה: 0.033 g • גירוסקופים: 0.21 °/sec

גרף:מסלול מחושב לפי קלט אמיתי • אורך מסלול: • 285 מטר • רעש משוערך (סטיית תקן): • מדי תאוצה: 0.089 g • גירוסקופים: 0.89 °/sec

הורדת סיבוכיות • סיבוכיות המערכת מושפעת ישירות ממימד מצב מורחב (xa) • שלב יצירת sigma points בעל סיבוכיות חישוב (~n3/3), n הוא מימד הווקטור • עבור רעש חיישנים אדיטיבי ניתן להוריד מימד הווקטור: • יוצרים sigma points לפי וקטור מצב מקורי (x) • הפעם נתוני חיישנים אינם חלק מהווקטור אלא פרמטרים לטרנספורמציה • רעש החיישנים מסתכם לקווריאנס המצב כשלב אחרון של חיזוי המצב • השיטה נקראת Additive UKF • הגענו לייצוג הנ''ל בשני שלבים

הורדת סיבוכיות - פרטים • מימוש בסיסי (תואר קודם) : • שלב 1 – רעש מדי תאוצה אדיטיבי : • שלב 2 – רעש גירוסקופים אדיטיבי :

גרף:השוואה בין מימושים - סיבוכיות • זמן חישוב(ב-Matlab): • 22.4 min • 19.4 min • 16.5 min • 22.4/16.5 = 1.35

גרף:מסלול מחושב לפי קלט עם שגיאות מוגברות • אורך מסלול: • 7.4 ק''מ • רעש משוערך (סטיית תקן): • מדי תאוצה: 0.105 g • גירוסקופים: 1.01 °/sec • מסלול אדום – עבור שגיאה מקורית • מסלול כחול – עבורשגיאה מוגברת • הבדל בין מסלולים (RMSE): • 29.5 [m]

גרף:השוואה בין מימושים - סיבוכיות • זמן חישוב(ב-Matlab): • 1.9 min • 1.6 min • 1.4 min • 1.9 / 1.4 = 1.35

גרף:מסלול מחושב לפי קלט עם שגיאות מוגברות • אורך מסלול: • 285 מטר • רעש משוערך (סטיית תקן): • מדי תאוצה: 0.13 g • גירוסקופים: 1.33 °/sec • מסלול אדום – עבור שגיאה מקורית • מסלול כחול – עבורשגיאה מוגברת • הבדל בין מסלולים (RMSE): • 0.25 [m]

מסקנות ביניים • השוואה למימוש בסיסי: • מימוש 2 (רעש תאוצה אדיטיבי) • שיפור זמן כ- 17% • שגיאה זהה • מימוש 3 (RBAUKF) • שיפור זמן כ- 35% • שגיאה קרובה • מסקנה: שיטת רעש אדיטיבי מאפשרת להוריד סיבוכיות המערכת ללא הפסד מהותי בדיוק השערוך

scaling factor • הוספנו התחשבות בשגיאת קנה מידה לכל אחד מהמימושים הקודמים • בדומה לשגיאת היסט • שומרים קנה מידה משוערך בווקטור מצב • עדכון נעשה יחד עם תיקון המצב • עוד 6 רכיבים לווקטור מצב, סה''כ 22 רכיבים • שינוי בחיזוי מצב:

גרף:השוואה בין מימושים – קנה מידה תאוצה

גרף:השוואה בין מימושים – קנה מידה גירוסקופים

מסקנות ביניים • מערכת רגישה יותר לשגיאת קנה מידה של גירוסקופים • מימושים ללא שיערוך קנה מידה: • מתנהגים דומה למצב של רעש מוגבר • מימושים עם שיערוך קנה מידה: • עבור ערכים קטנים של שגיאת קנה מידה • שגיאה גדלה • סדר מודל גדול מדי יכול לספק הפרש קטן בין מדידה אמיתית ומשוערכת גם עבור הבדל גדול במצב • עבור ערכים גדולים של שגיאת קנה מידה • שגיאה קטנה לעומת מימושים בסיסיים • שגיאה כמעט לא גדלה עם הגדלת ערך קנה מידה • שיערוך קנה מידה נעשה קל יותר

סיכום ומסקנות • מימשנו מערכת ניווט אינרציאלי מבוסס על RBUKF המסוגלת להתמודד עם רעשי חיישנים • ניתן להוריד סיבוכיות בעזרת שיטת רעש אדיטיבי (RBAUKF) • יש יתרון בשערוך שגיאת קנה מידה רק עבור ערכים גדולים שלה • שיפורים אפשריים: • בטרנספורמציה שמבצעים על sigma points יש חלק ליניארי (למשל תיקון היסט) שמאפשר הורדת סיבוכיות נוספת • ניתן לשקול הנחות מודל נוספות לשלב השוואת מדידה (כגון זווית roll קטנה)

תודה על הקשבה

ניווט אינרציאלי מבוסס על Unscented Kalman Filter