ניווט אינרציאלי מבוסס על UNSCENTED KALMAN FILTER

1. ניווט אינרציאלי מבוסס על UNSCENTED KALMAN FILTER מגישות: שירלי ליבנה 011949286 הילה זלוטו 034719625 מנחה: דר' גבי דוידוב

2. נושאי המצגת מבוא מטרת הפרויקט,הגדרת הבעיה, דרך פתרון ,שלבי הפרויקט רקע תיאורטי מבנה מערכת אינרציאלית סימולציה לכיול מעבר בין מערכות ,השפעת מהירות סיבוב כד"וא ,שלבים במימוש המערכת, הוספת רעשים והשפעתם , מימוש מסנן קלמן UKF סימולציה אמיתית סימולציה אמיתית- מימוש המסלולים, עמידות האלגוריתם –בדיקת ספי התבדרות עבור מסלולים אמיתיים סיכום ומסקנות תודות ביבלוגרפיה

3. מבוא

4. מטרת הפרויקט מציאת מיקום של  גוף אשר נמצא על מפה נתונה ע"י שימוש  במדידים אינרציאלים מדויקים  תוך שימוש במשערך UKf -Unscented Kalman Filter

5. הגדרת הבעיה • נתון רכב נוסע ממוכשר עם IMU= Inertial measurment unit שכולל 3 גירוסקופים ושלושה מדדי תאוצה , ותפוקות של מד מהירות גלגל(אודו מטר). • הבעיה היא לשערך את מיקום ואורינטצית הרכב במהלך הנסיעה(שיערוך המסלול)

6. דרך הפתרון יצירת מערכת המקבלת את תוצאות החיישנים(שלושה גירוסקופים, שלושה מדדי תאוצה- עבור כל אחד מהצירים Pitch,Roll, Yaw ) ופולסי אודומטר. המערכת מייצרת את גרף המסלול תוך כדי שימוש במסנן קלמן לתיקון הרעשים. • נציין שפולסי אודומטר אינם חלק אינטגרלי ממערכת הניווט האינרציאלית הבסיסית, אך לאור העובדה שכיול החיישנים לשם מזעור השגיאות מייקר את עלות ה IMU, הפתרון לכך הוא חסימת ההתבדרות ע"י סנסורים נוספים. במקרה שלנו זה מד המהירות גלגל האוטו-אודומטר.

7. שלבי הפרויקט • ביצוע סימולציה לכיול המערכת- דימינו נתוני חיישנים (מסלול) ויצרנו מערכת ישירה והפוכה. ללא הכנסת רעשים ציפינו שכאשר נעבור ממערכת הישירה להפוכה נקבל את אותם ערכים שהכנסנו. • קבלת מסלול אמיתי וע"י שימוש במערכת שבנינו שיערוך המסלול – מיקום ואוריינטצית הרכב

8. רקע תיאורטי

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

10. (IMU)מבנה מערכת אינרציאלית :

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

12. ג'ירוסקופ • נציין שקיימות מספר טכנולוגיות למדידת מהירות סיבוב יחסית לכדו"א.כאמור,אנחנו פירטנו על הסנסורים האלקטרו מכניים- המבוססים על דיסקה מסתובבת. • טכנולוגיות נוספות: • טכנולגיות אופטיות כגון FOG= Fiber Optic Gyro • טכנולגיית RLG=Ring Laser Gyro • MEMS- Micro electro Mechanical Systems • לכולם מטרה זהה- מדידת המהירות האינרציאלית של גוף.

13. מד תאוצה/אקסלומטר מד תאוצה, חד צירי . מורכב ממסה וקפיץ או ממטוטלת. קיימת משמעות רבה לאוריינטציה.

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

15. סימולציה לכיול

16. מעבר בין מערכות בדגש על השפעת כדו"א. תיאור התהליך- דיאגרמת בלוקים-

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

18. המשוואות הבאות קושרות בין מדידות הג'ירוסקופים למהירויות או- נציין שהנ"ל נכון רק לזוויות למוגבלות(עד כ ~ ) קרוב ל-90 מעלות הנ"ל מתפוצץ ואז צריך לעבור לקווטרניון.

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

20. BODY TO NED TRANSFORMATION-leveling "יישור" מדדי התאוצה

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

22. השפעת מהירות סיבוב כדור הארץ: מערכת כדוהא מרכזה במרכז המסה של כדוהא, ציריה קבועים בהתאם לכדוהא: * ציר X ממוקם לכיוון ממוצע המרידיאן של גריניץ (מרידיאן=קו דמיוני הנמתח על פני כדו"א מהקוטב הצפוני לקוטב הדרומי ומקשר את כל הנקודות בעלות אותו אורך גיאוגרפי.המרידיאן העובר דרך "מצפה הכוכבים המלכותי של גריניץ , בסמוך ללונדון, מוגדר קו אורך האפס או המרידיאן הראשי) * ציר Z מקביל לממוצע ציר הסיבוב של כדוהא * ציר Y משלים יד ימין אורתוגונלי של המערכת יחס הסיבוב של המערכת ביחס למערכת העצמית:

23. השפעת מהירות כדור הארץ: • ניתן לתאר את וקטור המיקום של כדוה"א במונחים של : • Rn- רדיוס העקמומיות במעגל אנכי • קו האורך :

24. השפעת גיאומטריית כדו"א הארץ – ערכים התחלתיים

25. השפעת גיאומטריית כדור הארץ – חישוב עבור כל נקודה:

26. השפעת כדור הארץ – סיכום בשלב מוקדם של הפרויקט עשינו סימולציה. כאשר אין השפעת כדו"א המסלול התקבל הוא מעגלי. ברגע שנוסיף את השפעת סיבוב כדו"א מתקבל מסלול לא מעגלי- בגלל נושא המהירות היחסית

27. שלבים במימוש המערכת מודלים שפותחו • IMU to NED (North East Down) • NED to IMU • רעשים מתווספים • סימולטור שיערוך מבוססUKF מודלים מוכנים שנעזרנו בהם • משערך UKF • רעשים מתווספים- מודלים קיימים מהMATLAB

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

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

30. ללא תוספת מהירות סיבוב כדור הארץ:

31. עם תוספת מהירות סיבוב כדור הארץ: ניתן לראות שהגרפים כבר לא מתלכדים כצפוי. הפתרון- שימוש במסנן קלמן.

32. מימוש מסנן קלמן UKF במקרה שלנו, המודל אינו לנארי, לכן לא ניתן להשתמש במסנן רגיל. קיימים שני סוגים שונים עקריים לסינון הרעש: א. Extended Kalman Filter ב. Unscented Kalman Filter

33. מסנן Extended Kalman Filter – למה לא?? • מבצע לינאריזציה של המודל • קשה למימוש(מבחינה מתמטית) • מחקרים מראים שמסנן קלמן זה הוא אינו המשערך האידיאלי,יש נטייה רבה לטעויות(שנגררות) • מאחר ומטריצת הקווריאנס נוטה שלא להעריך נכון את • הערכים של מטריצת הקווריאנס,לכן מסכנת • באופן משמעותי את חישובי הסטטיסטיקה לשם שיערוך • המצב הבא

34. מסנן Unscented Kalman Filter – מימשנו • כאשר המודל לא לינארי במיוחד ה EKF נותן תוצאות מאד לא מדויקות. הסיבה לכך היא שמטריצת השונות נוצרת דרך לינאריזציה לא המודל הלא לינארי(פיתוח לטור טיילור). • מטריצת השונות של ה UKF מבוססת על טכניקת דגימה דטרמינסטית- דגימת מס מינימלי של נקודות דגימה סביב הממוצע.(Sigma Points ) • צפיפות ההסתברויות משוערכת ע"י משתנים אקראים, מה שמחזיר ערכים הרבה יותר מדויקים מאשר פיתוח טיילור מסדר ראשון של הפונקציה הלא לינארית של EKF • הגירסא היותר יציבה של EKF (למעשה זה שידרוג של EKF )

35. מודל המערכת Z – ווקטור המדידה: תאוצה, מדידות הגירו', אורך המסלול,פולסי האודומטר X – ווקטור המצב P – מטריצת השונות המשותפת w – רעש המערכת Q– קווריאנס של רעש המערכת v – רעש לבן (רעש המדידה) R –קווריאנס של רעש המדידה

36. חיזוי ווקטור המצב: • יצירת סט וקטורי מצב על ידי הוספת ה- sigma pointsלוקטור מצב הנוכחי(xi(k-1)) • העברת וקטור של הסט xi(k-1) במודל לקבלת סט • (xi(k| k-1)) • 3. חישוב ממוצע וקווריאנס של הסטxi(k | k-1)

37. חיזוי וקטור המצב- משוואות עיקריות נגדיר: סט של 2L+1 נקודות דגימה :

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

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

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

41. עידכון וקטור המצב-משוואות עיקריות כמו קודם , סט של 2L+1 נקודות דגימה : נעביר את נקודות הדגימה דרך:Observation Function-h

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

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

44. תוצאות סימולציה תיאורתית- בלי מסנן קלמן • הוספת רעשים • ללא מסנן קלמן • עם התחשבות בתאוצת כדו"א

45. תוצאות סימולציה תיאורתית- עם מסנן קלמן • הוספת רעשים • עם מסנן קלמן • עם התחשבות במהירות סיבוב כדו"א ואכן נראה שקיימת התאמה כמעט מוחלטת עם שימוש במסנן קלמן

46. תוצאות סימולציה תיאורתית- עם ובלי מסנן קלמן • הוספת רעשים • ללא התחשבות במהירות סיבוב כדו"א עם שימוש במסנן קלמן ללא שימוש במסנן קלמן

47. סימולציה אמיתית

48. סימולציה אמיתית- מימוש מסלולים • התקבלו שני מסלולים של רכב נוסע. • עבור כל אחד מהמסלולים התקבלו 9 פרמטרים כמפורט: • ג'ירו ציר X [מעלות לשנייה] • ג'ירו ציר Y [מעלות לשנייה] • ג'ירו ציר Z [מעלות לשנייה] •  מד תאוצה ציר X [מעלות לשנייה] • מד תאוצה ציר Y [מעלות לשנייה] • מד תאוצה ציר Z [מעלות לשנייה] • אורך המסלול (על מנת לעבור ליחידות של מטר יש לכפול פי-0.3846) • פולסי אודומטר • זמן דגימה 0.005 שניות

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

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