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

' י אוסדצ איליהאוחרימנקו ודים

לישראל – טכנולוגי מכון הטכניוןחשמל להנדסת הפקולטה

ורובוטיקה לבקרה המעבדה

בהנחייתדוידוב'' גבי ר ד

הדיון נושאימוטיבציה•הפרויקט • מטרותאינרציאלי • ניווט•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)חיזוי עלקודם ) (uוחיישנים( )xk-1|k-1מצב

צפוי – פלט (yk|k-1)חישובנמדד – לפלט אודומטר( yk)השוואה מדידת כלומרמצב – תיקון

לרמת • בהתאם ומדידות מצב למשתני משקלים נותן המסנןבטחון

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

Unscented transformationוקטור • על ליניארית לא טרנספורמציה להפעיל דרוש

אקראי• : אפשרי Unscented transformationפיתרון

תוחלת – סביב נקודות בוחריםלקווריאנס בהתאם

“(sigma-points )”על – טרנספורמציה מפעילים

נקודה כלוקווריאנס – תוחלת מחשבים

שהתקבלו נקודות סמך עלמשקלים ) (Ws, Wcלפי

" בסיסי " מימושוחלק • ליניארי לחלק הבעיה את נחלק שלנו הבסיסי במימוש

ליניארי- (Rao-Blackwellised UKF)לאמצב )– - )fחיזוי פלט)(( וחיזוי ליניארי ליניארי)(( hלאמסובבת – צירים במערכת מהירות מצב בווקטור שומרים לכן

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

•: המצב וקטורתאוצה, (3)זוויות, (3)מהירות, (3)מיקום] גירוסקופים, (3)היסט [(3)היסט מרחק,

כ'' – רכיבים 16סה

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

Measurements

aBAccelerometers

Gyroscopes wB

Odometer dist

UKF

State Prediction

State

Sigma points generation

Transform(non-linear)

covariance

Combine

pred State

predicted covariance

Predict observation

observation

model assumptions

Calculate gain

-

UpdateStored route

sigma pointsיצירת •מורחב )– מצב ונתוני( augmentedיוצרים מצב מווקטור

'' כ ) סה (:L=22חיישנים רכיבים

:sigma pointsמחשבים –

הינה מטריצה על שורש פעולת Cholesky decompositionכאשר

– " פרטים " בסיסי מימוש

של • sigma pointsטרנספורמציהנקודה – כל χiעל

k-1|k-1 לא טרנספורמציה מפעיליםנקודה ומקבלים χiליניארית

k|k-1

– " פרטים " בסיסי מימוש

χiהחדשים )sigma pointsמתוך •k|k-1 )חיזוי מקבלים

: הבא המצב

מדידה • חיזוי מחשבים מצב חיזוי ^y)מתוךk|k-1 )

: ליניארית בצורה וקווריאנסים

•: מצב תיקון

– " פרטים " בסיסי מימוש

גרף:אמיתי קלט לפי מחושב מסלול

•: מסלול אורך7.4'' מ ק

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

גרף:אמיתי קלט לפי מחושב מסלול

•: מסלול אורךמטר 285

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

סיבוכיות הורדתמורחב • מצב ממימד ישירות מושפעת המערכת (xa)סיבוכיות

יצירת – חישוב )~sigma pointsשלב סיבוכיות הוא n3/3 ,)nבעלהווקטור מימד

•: הווקטור מימד להוריד ניתן אדיטיבי חיישנים רעש עבורמקורי )sigma pointsיוצרים – מצב וקטור (xלפיפרמטרים – אלא מהווקטור חלק אינם חיישנים נתוני הפעם

לטרנספורמציהחיזוי – של אחרון כשלב המצב לקווריאנס מסתכם החיישנים רעש

המצבנקראת – Additive UKFהשיטה

• '' שלבים בשני ל הנ לייצוג הגענו

פרטים - סיבוכיות הורדת•) קודם ) תואר בסיסי :מימוש

אדיטיבי – 1שלב • תאוצה מדי :רעש

אדיטיבי – 2שלב • גירוסקופים :רעש

גרף:סיבוכיות - מימושים בין השוואה

חישוב • זמן- (:Matlabב)

22.4 min19.4 min16.5 min

22.4/16.5 = 1.35

גרף:מוגברות שגיאות עם קלט לפי מחושב מסלול

•: מסלול אורך7.4'' מ ק

•:) תקן ) סטיית משוערך רעש: תאוצה g 0.105מדי/1.01גירוסקופים: °sec

שגיאה – אדוםמסלול • עבורמקורית

שגיאה – כחולמסלול • עבורמוגברת

מסלולים )• בין (:RMSEהבדל29.5[ m]

גרף:סיבוכיות - מימושים בין השוואה

חישוב • זמן- (:Matlabב)1.9 min1.6 min1.4 min

1.9 / 1.4 = 1.35

גרף:מוגברות שגיאות עם קלט לפי מחושב מסלול

•: מסלול אורךמטר 285

•:) תקן ) סטיית משוערך רעש: תאוצה g 0.13מדי/1.33גירוסקופים: °sec

שגיאה – אדוםמסלול • עבורמקורית

שגיאה – כחולמסלול • עבורמוגברת

מסלולים )• בין (:RMSEהבדל0.25[ m]

ביניים מסקנות•: בסיסי למימוש השוואה

(2מימוש – אדיטיבי ) תאוצה רעש• - כ זמן 17%שיפורזהה • שגיאה

(RBAUKF )3מימוש –• - כ זמן 35%שיפורקרובה • שגיאה

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

scaling factor

ב • התחשבות מידה הוספנו קנה לכל שגיאתהקודמים מהמימושים אחד

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

'' 6עוד • , כ סה מצב לווקטור רכיבים 22רכיבים•: מצב בחיזוי שינוי

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

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

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

מוגבר – רעש של למצב דומה מתנהגים•: מידה קנה שיערוך עם מימושים

ערכים – מידה קטניםעבור קנה שגיאת שלגדלה • שגיאה

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

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

יותר – קל נעשה מידה קנה שיערוך

ומסקנות סיכוםעל • מבוסס אינרציאלי ניווט מערכת RBUKFמימשנו

חיישנים רעשי עם להתמודד המסוגלתאדיטיבי )• רעש שיטת בעזרת סיבוכיות להוריד (RBAUKFניתןגדולים • ערכים עבור רק מידה קנה שגיאת בשערוך יתרון יש

שלה

•: אפשריים שיפוריםעל – שמבצעים ליניארי sigma pointsבטרנספורמציה חלק יש

) נוספת) סיבוכיות הורדת שמאפשר היסט תיקון למשלזווית ) – כגון מדידה השוואת לשלב נוספות מודל הנחות לשקול ניתן

roll) קטנה