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

תוכן  מטרת הפרויקט  מוטיבציה לביצוע הפרוייקט  מדידות ואפיון רעש המגנטומטר  אודומטריה  בניית הסימולטור  Kalman Filter  מסקנות

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

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

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

מדידות ואפיון רעש המגנטומטר - המשך Minute: Hour: רעש לבן במדידות לאורך דקה מגמה אקספוננציאלית במדידות לאורך שעה

אודומטריה נתוני התנועה מהרובוט ניתנים ע " י : θ L - נתוני סיבוב גלגל שמאלי θ R - נתוני סיבוב גלגל ימני כאשר : * כדי לחשב את θ L ואת θ R, בכל שלב מחשבים את d או ΔΨ. ΔΨ d b R left right θ R = 2d + ΔΨ *b, θ L = 2d - ΔΨ *b 2 R 2 R d = ( θ R + θ L ) R, ΔΨ = ( θ R - θ L ) R 2 b

בניית הסימולטור - כללי עבודת הסימולטור, בהינתן מסלול הרובוט בחדר נתון וזמן דגימה מהמשתמש : 1. שרטוט המסלול המבוקש. 2. יצירת מטריצה המחזיקה בכל שורה את נתוני האודומטריה עבור פרק הזמן המבוקש (בהתחשב בהאצה, תאוטה וכו'...). 3. שחזור המסלול המקורי בהנתן מטריצת האודומטריה עם אפשרות להוסיף רעש לשחזור, ובכך לדמות פעולת הרובוט. 4. יצירת שדה מגנטי לכל נקודת דגימה (המתקבלת מהשחזור הנ"ל) עם אפשרות להוסיף רעש למדידות. 5. שימוש באלגוריתם KALMAN FILTER בכדי לנסות לתקן את שגיאות הרובוט ושגיאות המגנטומטר.

בניית הסימולטור – חישוב התנועה בהינתן תאוצת הרובוט - a ומהירותו המקסימלית v max : v = ∫a dt s = ∫v dt = ½at² בכדי לחשב את d (מרחק) ואת ΔΨ (זווית הסיבוב) בכל פרק זמן : זמן הגעה עד מהירות מקסימלית: t max = v max /a המרחק שהרובוט עובר עד שהגיע ל- t max : s max = ½v max ²/a הזמן והמרחק הנ"ל זהים עבור תאוצה ותאוטה (t accelerate =t decleration). המרחק שעובר הרובוט ב-v max : s sat = s total -2s max = s total - v max ²/a

בניית הסימולטור - חישוב התנועה- המשך ניתן לחלק לשתיים את תנועת הרובוט : 1. תנועה בקו ישר. 2. תנועה סיבובית במקום. התנועה בקו ישר: מתחלקת ל-2 מקרים: א. זמן התנועה מספיק ארוך כך שתתחלק לתאוצה,תנועה במהירות קבועה ותאוטה ב. זמן התנועה קצר ולכן הרובוט רק מאיץ ומאט v t t max S sat זמן ל - S max זמן ל - תאוצה תאוטה

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

בניית הסימולטור – השדה המגנטי לאחר שחזור המיקום באמצעות נתוני האודומטריה, מוגדר שדה מגנטי בכל נקודת דגימה. מטריצת המעבר מהשדה בחדר לשדה הנמדד על ידי הרובוט: H_robot_x H_robot_y H_robot_z cos ψ sin ψ 0 -sin ψ cos ψ H_world_x H_world_y H_world_z = * המרת מערכות צירים

Kalman Filter - הקדמה בשנת 1960 פרסם R.E.Kalman את תוצאות מחקרו בנושא סינון רעשים. מטרת המסנן הנה להעריך את מצב המערכת המכילה רעשים שונים ולתקן את הערכים השגויים בצורה רקורסיבית. בהינתן הערכה על התנהגות המערכת ונתוני המדידות, ניתן לבצע שיערוך של הנתונים. גודל השערוך תלוי ב הערכת כמות הרעש במערכת. predict correct

Kalman Filter - מימוש x k = x k ’ + Δx k x k x k’ Δx k y k = y k ’ + Δy k => χ = y k χ’ = y k’ Δχ = Δy k Ψ k = Ψ k ’ + Δ Ψ k Ψ k, Ψ k’, Δ Ψ k Q - הערכה לקווריאנס של רעש התהליך (Δθ L, Δθ R ) P - הערכה לקווריאנס של שגיאת מדידת המקום ( Δχ) - מתוקן כל צעד R - הערכה לקווריאנס של רעש המגנטומטר Z – הפרש בין השדה המגנטי המדוד לשדה הניתן מחישובי הרובוט True values Computed values Errors

Kalman Filter – דוגמא *distance between original and kalman is : cm *distance between original and computed is : cm

מסקנות Kalman Filter יעיל מאוד בשיפור תנועת הרובוט. באמצעות הפרמטרים R ו -Q אנו שולטים במידת ההערכה לרעש הן במגנטומטר והן במדידות הרובוט : o ככל שמגדילים את R מעריכים רעש גדול יותר במגנטומטר. o ככל שמגדילים את Q מעריכים שגיאות גדולות יותר במדידות הרובוט. # בכדי להגיע לאופטימום עלינו לבצע כוונון עדין בין שני הפרמטרים.

המרה ממערכת החדר הרובוט המרה ממערכת החדר למערכת הרובוט y world ψ ψ k kxkx kyky lxlx lyly x world y robot x robot חזור