Transcript ניווט אינרציאלי

‫ניווט אינרציאלי באמצעות‬
‫‪Unscented Kalman Filter‬‬
‫מגישים‬
‫גיא רוזנטל‬
‫ניר בן זריהם‬
‫מנחה ד"ר גבי דוידוב‬
‫תאריך‪7.11.10 :‬‬
‫אביב תש"ע‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫הצגת הבעיה‬
‫בהיעדר אמצעי ניווט חיצוני (‪ )GPS‬ניתן לנווט בעזרת מערכת‬
‫ניווט עצמונית (‪ )IMU‬המותקנת על כלי הרכב‬
‫מערכת זו כוללת‪:‬‬
‫‪ 3 .1‬מדי תאוצה‬
‫‪ 3 .2‬מדי מהירות זוויתית (‪)GYRO‬‬
‫‪ – Odometer .3‬מד מהירות גלגלים‬
‫מתוך מדידות אלו וידיעת המיקום ההתחלתי‬
‫ניתן לשערך את מיקום הרכב בכל זמן‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫ניווט אינרציאלי ‪ -‬איך זה עובד?‬
‫הנחה‪ :‬בהינתן תאוצות בצירי הניווט (‪ (NED‬ניתן לחשב ע"י אינטגרציה‬
‫אחת את המהירויות באותם צירים‪ ,‬וע"י אינטגרציה נוספת לשערך את‬
‫המיקום במרחב‬
‫בעיה‪ :‬התאוצות הנמדדות ע"י ה‪ IMU -‬נתונות בצירי גוף‪ ,‬לכן יש צורך‬
‫להעבירן לצירי ניווט‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫טרנספורמציית הסיבוב‬
‫המצב הזוויתי מגדיר את סיבוב הגוף ביחס למערכת צירי הניווט ע"י‬
‫טרנספורמציית הסיבוב‪:‬‬
‫‪0‬‬
‫‪0  cos() 0  sin()  cos( ) sin( ) 0 ‬‬
‫‪1‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫‪  0 cos( ) sin( )  0‬‬
‫‪1‬‬
‫‪0   sin( ) cos( ) 0 ‬‬
‫‪ 0  sin( ) cos( )  sin() 0 cos() ‬‬
‫‪0‬‬
‫‪0‬‬
‫‪1 ‬‬
‫‪‬‬
‫‪‬‬
‫‪‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪DBODY  NED‬‬
‫לאחר שהומרו התאוצות לצירי ניווט‪ ,‬אינטגרציה כפולה עליהם תיתן את‬
‫המיקום במרחב‬
‫‪Position‬‬
‫‪‬‬
‫‪aNED‬‬
‫‪Body frame‬‬
‫‪To Navigation frame‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪abody‬‬
‫ניווט אינרציאלי – סכימת בלוקים‬
abody
rotation
aNED
Trasformation
IMU
Matrix
body
Attitude
Calculation
UKF ‫ניווט אינרציאלי באמצעות‬

Position
Velocity
Attitude
‫משוואות ניווט‬
‫כל מערכות הניווט האינרציאלי צומחות מתוך סט המשוואות הבא‪:‬‬
‫‪NED accelerations‬‬
‫‪Earth-NED rotation‬‬
‫‪Downward velocity‬‬
‫‪NED-BODY rotation‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫השפעות כדור הארץ‬
‫המצב הזוויתי של הרכב נמדד ע"י ‪ 3‬סביבונים‪ .‬סביבונים אלו מושפעים מסיבוב‬
‫כדה"א‪.‬‬
‫מידול כדה"א כספירה אליפטית מסתובבת גורר יחס תלוי מיקום בין מערכת צירי‬
‫הגוף למערכת צירי ניווט‪.‬‬
‫יש לתקן את מדידת הסביבונים תוך התייחסות למיקום הגיאוגרפי על פני כדה"א‬
‫לכן בווקטור המצב נשמרות קואורדינאטות המיקום על כדור הארץ‬
‫‪ .1‬האורך הגיאוגרפי‬
‫‪ .2‬הרוחב הגיאוגרפי‬
‫‪ .3‬גובה מעל פני הים‬
‫‪‬‬
‫‪‬‬
‫‪h‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫תיקוני השפעת כדה"א‬
‫ערוץ הסביבונים‬
    DBLLLI dt
LLI
  cos    cos  







    sin    sin   


UKF ‫ניווט אינרציאלי באמצעות‬
‫תיקוני השפעת כדה"א‬
‫ערוץ מדי התאוצה‬

VN  aN   VD

VN
VE
 (2 
)VE sin  
RN  h
( RE  h) cos 




VE
)(VN sin   VD cos  ) 
VE  aE   (2 
( RE  h) cos 




VN
VE
2h
 (2 
)VE cos    g (1  0.0052884sin 2  )(1  )
VD  aD   VN
RN  h
( RE  h) cos 
Re


RN * - radius of curvature in the prime vertical
UKF ‫ניווט אינרציאלי באמצעות‬
‫בעיית הרעש‬
‫•‬
‫הנחה‪ :‬במצב של כניסות אידיאליות‪ ,‬ניתן לחשב במדויק את מסלול‬
‫הגוף ע"י שימוש במודל הפיזיקאלי‬
‫•‬
‫בעיה‪ :‬בגלל שהמדידות טבולות ברעשים‪ ,‬המסלול שיחושב בעזרת‬
‫המודל הפיזיקאלי יתבדר במהירות בהשוואה למסלול האמיתי‬
‫•‬
‫פתרון‪ :‬כדי לקבל תוצאות נאמנות למציאות יש להכניס אמצעי‬
‫בקרה לאלגוריתם‪ ,‬לכן משתמשים ב ‪Kalman Filter‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪Kalman Filter‬‬
‫•‬
‫‪ Kalman Filter‬הוא משערך הנעזר במדידות חיצוניות (אובזרבלים) כדי‬
‫להעריך את שגיאת החיזוי שלו ולעדכן את ווקטור המצב בהתאם‬
‫•‬
‫בגלל שהטרנספורמציה לחישוב המצב הבא אינה ליניארית‪ ,‬יש להשתמש‬
‫במסנן מתאים‪:‬‬
‫‪ - Extended Kalman Filter‬קידום ווקטור המצב נעשה בצורה‬
‫אנליטית‪ ,‬ע"י ליניאריזציה מסדר ראשון של המערכת‬
‫)‪ -Unscented Kalman Filter (UKF‬מבצע שערוך סטטיסטי של‬
‫המודל עד סדר שני (מתבסס על ההנחה שקל יותר לשערך התפלגות‬
‫הסתברותית מאשר לשערך מודל לא ליניארי)‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫חיזוי המצב הבא‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫חיזוי האובזרוול‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫תיקון החיזוי ע"י מדידת האובזרוול‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫ סכימת בלוקים‬- UKF
Old state
Create prediction for
next state
Fix prediction in
accordance to extra
measurement
Create sigma points
Propagate each sigma
point through the
nonlinear model
Next state
UKF ‫ניווט אינרציאלי באמצעות‬
‫שלבי הפרויקט‬
‫א‪ .‬בניית סימולטור‪ :1‬חישוב תפוקות אידיאליות (מדי תאוצה וסביבונים) מתוך הזויות‬
‫שמצייר גוף המתקדם במרחב ומהירותו הקרקעית‬
‫‪‬‬
‫‪V‬‬
‫‪‬‬
‫‪simulator1‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪V‬‬
‫שלבי הפרויקט‬
‫ב‪ .‬בניית הסימולטור ההפוך‪ :‬מבוסס על הטרנספורמציות ההפוכות מסימולטור‪.1‬‬
‫מחשב מסלול התקדמות מתוך תפוקות ה‪( IMU-‬מדי תאוצה וסביבונים)‬
‫‪R‬‬
‫‪Reverse‬‬
‫‪simulation‬‬
‫‪‬‬
‫‪V‬‬
‫‪‬‬
‫‪simulator1‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪V‬‬
‫שלבי הפרויקט‬
‫ג‪ .‬הרעשת התפוקות של סימולטור ‪ .1‬בשלב זה רואים כי הסימולטור ההפוך נכשל‬
‫במשימת הניווט‪ -‬יש צורך בסינון‬
‫) ‪N ( a , Ea‬‬
‫‪R‬‬
‫‪Reverse‬‬
‫‪simulation‬‬
‫‪‬‬
‫‪V‬‬
‫‪‬‬
‫‪simulator1‬‬
‫) ‪N (  , E‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪V‬‬
‫שלבי הפרויקט‬
‫ד‪ .‬שיפור תוצאות ע"י הכנסת המסנן במקום הסימולטור ההפוך‬
‫) ‪N ( a , Ea‬‬
‫‪R‬‬
‫‪Reverse‬‬
‫‪simulation‬‬
‫‪R‬‬
‫‪UKF‬‬
‫‪‬‬
‫‪V‬‬
‫‪‬‬
‫‪simulator1‬‬
‫) ‪N (  , E‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪V‬‬
‫תוצאות‬
‫‪.1‬‬
‫כחול‪ -‬מסלול אמיתי‬
‫‪.2‬‬
‫אדום‪ -‬מסלול משוערך עם ‪UKF‬‬
‫‪.3‬‬
‫ירוק‪ -‬מסלול ללא סינון רעשים‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫תוצאות‬
‫‪500‬‬
‫‪real‬‬
‫‪predicted‬‬
‫‪0‬‬
‫‪-500‬‬
‫‪-1000‬‬
‫‪North‬‬
‫‪-1500‬‬
‫‪-2000‬‬
‫‪-2500‬‬
‫‪1200‬‬
‫‪1000‬‬
‫‪800‬‬
‫‪600‬‬
‫‪East‬‬
‫‪400‬‬
‫‪200‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪0‬‬
‫‪-3000‬‬
‫תוצאות‬
‫חסינות לרעש‬
‫‪500‬‬
‫שחור –מסלול אמיתי‬
‫‪1e0 noise‬‬
‫‪1e-1 noise‬‬
‫‪1e-2 noise‬‬
‫‪no noise‬‬
‫כחול‪ -‬רעש מסדר גודל של הכניסה‬
‫‪0‬‬
‫‪-500‬‬
‫‪1‬‬
‫אדום‪ -‬רעש מסדר גודל של ‪ 10‬מהכניסה‬
‫מהכניסה‬
‫‪-1500‬‬
‫‪-2000‬‬
‫‪-2500‬‬
‫‪1200‬‬
‫‪1000‬‬
‫‪800‬‬
‫‪400‬‬
‫‪600‬‬
‫]‪East[m‬‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫‪200‬‬
‫‪0‬‬
‫‪-3000‬‬
‫‪-200‬‬
‫]‪North[m‬‬
‫ירוק‪ -‬רעש מסדר גודל של‬
‫‪1‬‬
‫‪100‬‬
‫‪-1000‬‬
‫מסקנות‬
‫•‬
‫אלגוריתם ה‪ UKF -‬הוכיח את יעילותו‬
‫•‬
‫האלגוריתם גמיש ומאפשר כיול עדין בהתאם לסוג הבעיה‬
‫•‬
‫מודל כדה"א לא הראה שיפור משמעותי עבור המסלולים הנתונים‬
‫•‬
‫מודל כד"הא הראה חסינות רבה לרעשים‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬
‫הצעות למחקר נוסף‬
‫•‬
‫הוספת אובזרוול מד גובה ברומטרי‬
‫•‬
‫עדכון עצירה מוחלטת‬
‫•‬
‫בדיקת האלגוריתם עבור מסלול עם טווח השתנות בקנה מידה ארצי‬
‫ניווט אינרציאלי באמצעות ‪UKF‬‬