ArogetiShai

‫בקרה של כלי רכב אוטונומיים‬ ‫מחקר לשם מילוי חלקי של הדרישות לקבלת תואר "דוקטור לפילוסופיה"‬ ‫מאת‬ ‫שי ארוגטי‬ ‫הוגש לסנאט אוניברסיטת בן‪-‬גוריון בנגב‬ ‫דצמבר ‪2005‬‬ ‫כסלו תשס"ו‬ ‫באר שבע‬ ‫בקרה של כלי רכב אוטונומיים‬ ‫מחקר לשם מילוי חלקי של הדרישות לקבלת תואר "דוקטור לפילוסופיה"‬ ‫מאת‬ ‫שי ארוגטי‬ ‫הוגש לסנאט אוניברסיטת בן‪-‬גוריון בנגב‬ ‫אישור המנחה _____________‬ ‫אישור דיקן בית הספר ללימודי מחקר מתקדמים ע"ש קרייטמן _____________‬ ‫דצמבר ‪2005‬‬ ‫כסלו תשס"ו‬ ‫באר שבע‬ ‫העבודה נעשתה בהדרכת‪:‬‬ ‫דר' נדב ברמן‬ ‫‪.‬‬ ‫המחלקה להנדסת מכונות‬ ‫הפקולטה להנדסה‬ ‫‪.‬‬ ‫תודות‬ ‫תודה ל‪ -‬דר' נדב ברמן על ההנחיה המסורה‪ ,‬הזמן הרב שהקדיש לי‪ ,‬והידע‬ ‫והניסיון שחלק עימי במהלך עבודת המחקר‪.‬‬ ‫תודה לאשתי מירב וילדי עופר ורון על הסבלנות הרבה והתמיכה לאורך‬ ‫כל הדרך‪.‬‬ ‫תודה למשפחתי‪ ,‬להורי נורית ויצחק‪ ,‬ולהורי אשתי נחמה וצבי על העזרה‬ ‫הרבה‪.‬‬ ‫תוכן עניינים‬ ‫עמודים‬ ‫תקציר ‪ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬א‬ ‫רשימת סימנים ‪ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬ה‬ ‫‪...............................................‬‬ ‫‪ -1‬הקדמה‬ ‫‪ -1.1‬רקע ‪. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -1.2‬סקר ספרות ‪. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -1.3‬תרומת העבודה‬ ‫‪1‬‬ ‫‪1‬‬ ‫‪6‬‬ ‫‪16 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -2‬תכנון בקרים לכלי רכב אוטונומיים המבוססים על שימוש במודל שרשרת ‪19 .‬‬ ‫‪ -2.1‬פיתוח בקר למערכות הניתנות לייצוג בצורת שרשרת ‪19 . . . . . . . . . .‬‬ ‫‪-2.2‬‬ ‫תכנון בקרים לכלי רכב אוטונומיים בגישה של מערכת קואורדינאטות‬ ‫קרטזית קבועה במרחב ‪27 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪-2.3‬‬ ‫תכנון בקר לכלי רכב אוטונומי בגישה של מערכת קואורדינאטות‬ ‫פולארית קבועה במרחב ‪. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪70‬‬ ‫‪-2.4‬‬ ‫תכנון בקרים לכלי רכב אוטונומיים בגישה של מערכת קואורדינאטות‬ ‫‪.........................................‬‬ ‫מקומית‬ ‫‪78‬‬ ‫‪-2.5‬‬ ‫תכנון בקרים לכלי רכב אוטונומיים בגישה האינווריאנטית ‪95 . . . . . . .‬‬ ‫‪ -3‬פיתוח בקר לכלי רכב בעל ארבעה גלגלים המבוסס על מודל עם זוויות‬ ‫החלקה ‪101 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -3.1‬מודל שגיאות של כלי רכב עם זוויות החלקה במערכת קואורדינאטות‬ ‫קרטזית קבועה במרחב ‪103 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪-3.2‬‬ ‫פיתוח בקר המבוסס על מודל קינמטי של כלי רכב עם זוויות החלקה‬ ‫‪106‬‬ ‫‪ -4‬מימוש ופתרון בעיות מעשיות ‪123 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -4.1‬מימוש מסנן ‪124 . . . . . . . . . . . . . . . . . . . . . . . . . . . . Kalman Filter‬‬ ‫‪ -4.2‬מימוש בקר המופעל בגישה של מערכת קואורדינאטות קרטזית‬ ‫‪130 . . . . . . . . . . . . . . . .. . . . . . . . . . . . . . . . . . . . .‬‬ ‫קבועה במרחב‬ ‫‪ -4.3‬מימוש בקר המופעל בגישה של מערכת קואורדינאטות מקומיות‬ ‫‪140‬‬ ‫נספחים ‪143 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -A1‬תיאור מערכת הניסוי ‪143 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -A2‬תכנון מסלול רצוי המתאים לבקרים בגישה של מערכת קואורדינאטות‬ ‫קרטזית קבועה במרחב ‪146 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪ -A3‬תכנון מסלול רצוי המתאים לבקרים בגישה של מערכת קואורדינאטות‬ ‫מקומית‬ ‫מקורות‬ ‫‪149 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫‪154 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .‬‬ ‫תקציר‬ ‫עבודה זו עוסקת בתכנון ומימוש של מערכות בקרה בכלי רכב אוטונומיים‪ .‬היעוד‬ ‫של האלגוריתמים המוצגים בעבודה הוא ‪ ,path-following‬כלומר נהיגה אוטונומית‬ ‫לאורך מסלול רצוי‪ .‬באופן מצומצם נדונים גם נושאים הקשורים ב‪-‬‬ ‫‪trajectory‬‬ ‫‪ tracking‬כאשר במקרה זה מלבד נהיגה לאורך מסלול גיאומטרי יש לקיים גם חוק‬ ‫זמן מסוים‪ .‬הגישה המוצעת מבוססת על תכנון בקר )כללי( למערכת ‪ n‬ממדית אשר‬ ‫ניתן לייצג את המודל הקינמטי שלה ע"י מודל בצורת שרשרת )‪.(chained system‬‬ ‫הבקר המוצע במקרה זה הינו בקר לא ליניארי ויתרונותיו על פני בקרים אחרים‬ ‫המיועדים לפתרון בעיית ה‪ path-following -‬בגישה המבוססת על שימוש במודל‬ ‫שרשרת הם היותו חלק )בניגוד לבקרים אחרים מסוג ‪ (sliding modes‬והאפשרות )ע"י‬ ‫בחירה של פונקציה לא ליניארית מתאימה( למנוע צורך באותות בקר גדולים כאשר‬ ‫כלי הרכב נמצא רחוק מהמסלול הרצוי )בניגוד לבקרים ליניאריים(‪ .‬בנוסף לפיתוח‬ ‫הבקר הכללי מוצג כיצד ניתן לעשות בו שימוש לצורך פתרון בעיית ה‪-‬‬ ‫‪path‬‬ ‫‪ following‬במגוון גדול של אפשרויות הנובע משימוש בכלי רכב שונים‪ ,‬מהגדרות‬ ‫שונות של שגיאות כלי הרכב ביחס למסלול הרצוי ומפיתוח מודל השגיאות ביחס‬ ‫למערכות קואורדינאטות קבועות במרחב או מקומיות‪.‬‬ ‫עיקר העבודה מיועד לתכנון ומימוש של מערכות בקרה בכלי רכב בעל ארבעה‬ ‫גלגלים עם הנעה אחורית והיגוי קדמי‪ ,‬למרות זאת הגישה הכללית המבוססת על‬ ‫שימוש במודל שרשרת מתאימה גם לכלי רכב אחרים‪ ,‬לדוגמה מוצג בעבודה תכנון‬ ‫בקר על פי עקרונות דומים גם לכלי רכב בעל הנעה דיפרנציאלית )לכלי רכב כזה שני‬ ‫גלגלים מניעים ופעולת ההיגוי מבוצעת על ידי סיבוב כל אחד מהגלגלים המניעים‬ ‫במהירות סיבובית שונה(‬ ‫חלוקה נוספת )לא ע"פ סוג כלי הרכב( של הבקרים בעבודה קשורה במערכת‬ ‫הקואורדינאטות אשר משמשת למדידת מצב כלי הרכב והמסלול הרצוי‪ .‬במקרה‬ ‫הראשון נדונים בקרים המבוססים על שימוש במערכת קואורדינאטות קבועה‬ ‫במרחב )מערכת אינרציאלית(‪ ,‬בקרים כאלה מתאימים לשימוש כאשר מצב כלי‬ ‫הרכב נמדד ע"י חישנים כגון ‪ INS ,DGPS , GPS‬ומצפן מגנטי‪ .‬במקרה השני מדובר‬ ‫במערכת קואורדינאטות מקומית )מערכת שאינה קבועה במרחב( אשר רתומה לכלי‬ ‫הרכב או צמודה למסלול הרצוי‪ .‬חיישנים נפוצים במקרה זה הם מצלמת ווידאו‬ ‫וסורק לייזר אשר מותקנים על כלי הרכב ומודדים את המצב היחסי בינו ובין‬ ‫המסלול הרצוי‪ .‬מדידת המצב של כלי הרכב במרחב )לדוגמא מדידת ‪ (GPS‬אינה‬ ‫נחוצה במקרה זה‪.‬‬ ‫א‬ ‫ייצוג מודל כלי הרכב בצורת שרשרת מתקבל ע"י שימוש בטרנספורמציה של משתני‬ ‫המצב ואות בקרה מתאים )‪ .(feedback linearization‬ניתן למצוא בספרות שתי‬ ‫טרנספורמציות כאלה‪ ,‬אחת מבוססת על שימוש במערכת קואורדינאטות קבועה‬ ‫במרחב וסובלת ממצב סינגולארי כאשר כלי הרכב ניצב לציר ‪ .x‬השנייה מבוססת על‬ ‫מערכת קואורדינאטות צמודה למסלול הרצוי‪ .‬מלבד הטרנספורמציות האלה‬ ‫המשמשות כאן לבחינה ומימוש הבקר הכללי בכלי רכב מוצגות טרנספורמציות‬ ‫נוספות וייחודיות לעבודה זו‪ .‬במקרה של מערכת קואורדינאטות קבועה במרחב‬ ‫מוצגת טרנספורמציה נוספת אשר בה המצב הסינגולארי הקשור בכיוון כלי הרכב‬ ‫ביחס לציר ‪ x‬מוחלף במצב סינגולארי אחר הקשור בכיוון כלי הרכב ביחס למסלול‬ ‫הרצוי‪ .‬אחד מהיתרונות של הטרנספורמציה החדשה הוא העובדה שהמצב‬ ‫הסינגולארי לא תלוי בבחירה השרירות של מערכת הצירים‪ .‬טרנספורמציות נוספות‬ ‫המוצגות וייחודיות לעבודה זו הן‪ :‬טרנספורמציה המבוססת על שימוש במערכת‬ ‫קואורדינאטות פולארית קבועה במרחב )שימוש במערכת קואורדינאטות פולארית‬ ‫לא בהקשר של מודל שרשרת אינו ייחודי לעבודה זו( וטרנספורמציה המבוססת על‬ ‫מערכת קואורדינאטות הרתומה לכלי הרכב‪ .‬כל הטרנספורמציות כוללות מצבים‬ ‫סינגולאריים ולכן מאפשרות יציבות מקומית בלבד‪ .‬חלק מהבקרים המוצגים כוללים‬ ‫גם פיתוח של תנאים מספיקים ליציבות מקומית‪ ,‬הקשורים במצב ההתחלתי של‬ ‫כלי הרכב‪ ,‬בתכונות המסלול הרצוי ובתכונת ההתכנסות האקספוננציאלית של מודל‬ ‫השרשרת עם הבקר הכללי‪ .‬רעיון ייחודי נוסף המוצג בעבודה ונותן מסגרת אחת‬ ‫לכל הטרנספורמציות השונות )למודל שרשרת(‪ ,‬מבוסס על תכנון בקר איווריאנטי‬ ‫כללי מאוד אשר ניתן לגזור ממנו את כל שאר הבקרים כמקרים פרטיים‪.‬‬ ‫פיתוח המודלים הקינמאטים של כלי הרכב הנדונים בעבודה מבוסס בדרך כלל על‬ ‫הנחה של "גלגול ללא החלקה"‪ .‬על פי הנחה זו המהירות הצידית )‪ (lateral‬של כל‬ ‫גלגל בכלי הרכב )מהירות ביחס לציר ניצב לגלגל( שווה אפס‪ .‬למעשה בכלי רכב‬ ‫מהסוגים הנדונים‪ ,‬כאשר מסלול הנסיעה אינו קו ישר‪ ,‬קיימות תמיד זוויות החלקה‬ ‫אשר גודלן יחסי לכוח הצידי הפועל על הגלגל‪ .‬זוויות החלקה אלה עשויות להיות‬ ‫בעלות משקל בתנועת כלי הרכב כאשר הוא מבצע תמרונים במהירויות גבוהות‪.‬‬ ‫במהירויות נסיעה נמוכות כאשר זוויות ההחלקה קטנות ניתן לעשות שימוש בהנחה‬ ‫של "גלגול ללא החלקה"‪ .‬טענה זו מבוססת על הביצועים הטובים אשר התקבלו‬ ‫מהפעלת הבקרים המוצגים בעבודה במסגרת של ניסויים וסימולציות )המבוססות‬ ‫על שימוש במודל דינאמי(‪ .‬כאשר כלי הרכב נדרש לבצע תמרונים במהירויות‬ ‫גבוהות אשר גוררות זוויות החלקה גדולות לא ניתן עוד לעשות שימוש בהנחה של‬ ‫"גלגול ללא החלקה" )ללא פגיעה משמעותית בביצועים( והתכנון מצריך שימוש‬ ‫ב‬ ‫במודל אשר כולל בתוכו גם זוויות החלקה‪ .‬המודל המקובל לשימוש במקרה זה הינו‬ ‫מודל דינאמי‪ ,‬אולם בגלל היותו מורכב ולא ליניארי רוב הבקרים אשר פותחו עד כה‬ ‫והתבססו על מודל דינאמי הניחו מהירות נסיעה קבועה וזוויות היגוי קטנות‪ .‬הנחות‬ ‫אלה הן בסיס לליניאריזציה של המודל ותכנון בקר במסגרת של בקרה ליניארית‪.‬‬ ‫כאשר מדובר בכלי רכב הנעים בכבישים מהירים )ללא סיבובים חדים( הנחות כאלה‬ ‫בדרך כלל מספיקות ומאפשרות ביצועים טובים‪ .‬היעד של האלגוריתמים המוצגים‬ ‫בעבודה זו הוא מימוש בכלי רכב הנעים בתנאי שדה )‪ (outdoor‬לא בהכרח כבישים‬ ‫מהירים‪ ,‬לכן הנחות אלה אינן מתאימות כאן‪ .‬בעבודה זו מוצגת גישה ייחודית לפיה‬ ‫מפותח מודל קינמאטי אשר אינו מבוסס על הנחה של גלגול ללא החלקה‪ .‬פיתוח‬ ‫המודל מבוסס על לינאריזציה סביב המסלול הרצוי ואינו מצריך הנחות כגון זוויות‬ ‫היגוי קטנות או מהירות קבועה‪ .‬המודל המתקבל כולל חלק אחד אשר זהה למודל‬ ‫בצורת שרשרת )המבוסס על הנחה של גלגול ללא החלקה( וחלק נוסף לא ידוע‪,‬‬ ‫אשר מהווה "הפרעה" )חלק זה של המודל תלוי בזוויות ההחלקה(‪ .‬הרעיון מאחורי‬ ‫תכנון הבקר למודל זה מבוסס על הרצון להקטין ככול האפשר את ההשפעה של‬ ‫ההפרעה )החלקות( על השגיאות של כלי הרכב ביחס למסלול הרצוי‪ .‬שיטת התכנון‬ ‫במקרה זה מבוססת על מזעור קריטריון ‪ peak-to-peak‬ופתרון של אי שוויוניים‬ ‫מטריציונים ליניאריים )‪ .(LMI‬מתוך השוואה של בקר זה בתנאי סימולציה )עם מודל‬ ‫דינאמי הכולל זוויות החלקה( מול בקר אחר )המספק ביצועים דומים כאשר שניהם‬ ‫מופעלים עם מודל קינמטי ללא החלקות( מתקבל יתרון ברור לבקר אשר תוכנן‬ ‫בשיטה המוצעת‪.‬‬ ‫אחד הקווים המנחים של עבודת המחקר הוא פיתוח אלגוריתמיים אשר יהיו‬ ‫ישימים ומתאימים למימוש בכלי רכב‪ .‬לצורך השגת מטרה זו נבנתה מערכת ניסוי‬ ‫המבוססת על כלי רכב חשמלי בעל ארבעה גלגלים ופותחו כלים נוספים הנחוצים‬ ‫לצורך מימוש‪ .‬כלים אלה קשורים בתכנון מסלול רצוי‪ ,‬שינוי מערכות‬ ‫קואורדינאטות בזמן אמת ועיבוד המידע המתקבל מהחיישנים‪ .‬באופן כללי המודל‬ ‫הקינמטי והדינאמי של כלי רכב בעל ארבע גלגלים הינו לא ליניארי‪ ,‬לכן הדרך‬ ‫המקובלת לסינון ושיערוך של משתני מצב במקרה זה מבוססת על שימוש במסנן‬ ‫קלמן מורחב )‪ .(EKF‬בעבודה זו מוצגת גם גישה שונה המבוססת על פיתוח מודל‬ ‫ליניארי תלוי בזמן )ללא קירוב( ושימוש בכלים המיועדים לסינון ושיערוך במערכות‬ ‫ליניאריות‪.‬‬ ‫מימוש הבקרים המוצגים בעבודה זו )בדרך כלל גם בעבודות תיאורטיות אחרות(‬ ‫מצריך מסלול רצוי הניתן לייצוג באופן אנליטי‪ .‬גישה זו אינה "נוחה" למימוש בתנאי‬ ‫שדה‪ .‬גישה אחרת )יותר נוחה למימוש( מבוססת על ייצוג המסלול הרצוי ע"י סידרה‬ ‫ג‬ ‫של נקודות דרך )‪ (way points‬רצויות‪ ,‬אשר נקבעות על סמך מדידות או מודל של‬ ‫השטח )מפה‪ ,‬תצ"א(‪ .‬כדי להתאים בין הגישות השונות פותחו במסגרת עבודה זו‬ ‫אלגוריתמים המאפשרים תכנון מסלול‪ ,‬העובר דרך סדרה של נקודות דרך רצויות‬ ‫ומקיים תכונות גיאומטריות המאפשרות הפעלה של מערכות הבקרה ונסיעה חלקה‬ ‫של כלי הרכב‪ .‬תכנון המסלול בדרך המוצעת אפשרי גם בזמן נסיעה אוטונומית של‬ ‫כלי הרכב )‪.(real time‬‬ ‫חלק ניכר מתוך התוצאות התיאורטיות המוצגות בעבודה נבדק בתנאי סימולציה‬ ‫וחלקים מסוימים גם בתנאי ניסוי‪ .‬תוצאות מתוך סימולציות וניסויים אלה מוצגות‬ ‫בעבודה וממחישות את יעילות השיטות‪.‬‬ ‫מילות מפתח‪:‬‬ ‫כלי רכב אוטונומיים‪ ,‬רובוטים ניידים‪ ,‬מודל שרשרת‪ ,‬עקיבה אחרי מסלול‪ ,‬בקרה לא‬ ‫ליניארית‪ ,‬תכנון מסלול‪.‬‬ ‫ד‬ ‫רשימת סימנים‬ ‫סימן‬ ‫‪A‬‬ ‫תיאור‬ ‫ מטריצה המשמשת לייצוג מערכת בחוג סגור במרחב המצב‪,‬‬‫ומוגדרת ע"י ) ‪A = ( A + B2 K‬‬ ‫) ‪c (t‬‬ ‫‪C‬‬ ‫ קיצור‪ ,‬מוגדר ע"י ) ) ‪. c ( t ) = cos ( θ ( t‬‬‫מטריצה המשמשת לייצוג מערכת בחוג סגור במרחב המצב‪,‬‬ ‫ומוגדרת ע"י ) ‪. C = ( C + DK‬‬ ‫‪Cα‬‬ ‫‪ -‬מקדם חיכוך )‪ (Cornering stiffness‬של גלגל‪.‬‬ ‫‪Cα1‬‬ ‫‪ -‬מקדם החיכוך )‪ (Cornering stiffness‬של הגלגלים האחוריים‪.‬‬ ‫‪Cα 2‬‬ ‫‪r‬‬ ‫‪d‬‬ ‫ מקדם החיכוך )‪ (Cornering stiffness‬של הגלגלים הקדמיים‪.‬‬‫‪ -‬ווקטור המחבר את הנקודות ) ‪ ( x c , yc‬ו‪. ( x s , ys ) -‬‬ ‫)‪d(t‬‬ ‫ מרחק כלי הרכב מהנקודה בה נמדד המסלול הרצוי )גודל‬‫‪r‬‬ ‫הווקטור ‪.( d‬‬ ‫) ‪dα ( t‬‬ ‫‪ -‬הפרעה הנובעת מהחלקות של הגלגלים‪.‬‬ ‫) ‪e( t‬‬ ‫‪ -‬ווקטור משתני המצב ‪. ei‬‬ ‫) ‪e0 ( t‬‬ ‫‪ -‬שגיאה במיקום של כלי הרכב לאורך ציר ‪) x‬מוגדרת עבור בקר‬ ‫) ‪e1 ( t‬‬ ‫המיועד ל‪.(trajectory tracking -‬‬ ‫‪ -‬שגיאה במיקום של כלי הרכב‪.‬‬ ‫) ‪e2 ( t‬‬ ‫‪ -‬שגיאה בכיוון של כלי הרכב‪.‬‬ ‫) ‪e3 ( t‬‬ ‫‪ -‬שגיאה בכיוון הגלגלים הקדמיים של כלי הרכב‪.‬‬ ‫) ‪ei ( t‬‬ ‫‪ -‬משתני מצב במודל שרשרת‪.‬‬ ‫)‪e (t‬‬ ‫‪ -‬ווקטור משתני המצב ‪ , ei‬מלבד ‪. en‬‬ ‫)‪e (t‬‬ ‫‪ -‬מוגדר ע"י ) ‪e ( t ) " eT ( t ) Pe ( t‬‬ ‫)‪f (x‬‬ ‫‪ -‬ייצוג מסלול רצוי ע"י פונקציה במערכת צירים קרטזית‬ ‫) ‪f ( x, t‬‬ ‫)אינרציאלית(‪.‬‬ ‫ ייצוג מסלול רצוי ע"י פונקציה במערכת צירים קרטזית מקומית‬‫)צמודה לכלי הרכב(‪.‬‬ ‫ה‬ ‫)‪fx (s) , fy (s‬‬ ‫‪ -‬פונקציות המשמשות להגדרת מסלול רצוי באופן פרמטרי‪.‬‬ ‫) ‪Fy ( t‬‬ ‫‪ -‬כוח הפועל על הגלגל בכיוון ניצב לגלגל )כוח צידי(‪.‬‬ ‫) ‪Fy1 ( t‬‬ ‫‪ -‬כוח הפועל על גלגל אחורי בכיוון ניצב לגלגל )כוח צידי(‪.‬‬ ‫) ‪Fy2 ( t‬‬ ‫‪ -‬כוח הפועל על גלגל קדמי בכיוון ניצב לגלגל )כוח צידי(‪.‬‬ ‫) ‪Fx1 ( t‬‬ ‫‪ -‬כוח הפועל על גלגל אחורי בכיוון הגלגל )כוח אורכי(‪.‬‬ ‫) ‪Fx2 ( t‬‬ ‫‪ -‬כוח הפועל על גלגל קדמי בכיוון הגלגל )כוח אורכי(‪.‬‬ ‫‪I zz‬‬ ‫‪I j , Im‬‬ ‫ מומנט אינרציה של כלי רכב ביחס לציר הניצב למישור ) ‪( x , y‬‬‫‪ -‬מטריצות יחידה בגודל ‪ j × j‬או ‪ m × m‬בהתאמה‪.‬‬ ‫‪j‬‬ ‫‪ -‬מימד הווקטור ) ‪. z ( t‬‬ ‫‪L‬‬ ‫ אורך כלי רכב בעל ארבעה גלגלים )המרחק בין הסרן האחורי‬‫והסרן הקדמי(‬ ‫‪l1 , l 2‬‬ ‫ אורכים המגדירים את מיקום מרכז המסה של כלי הרכב‬‫) ‪.( L = l1 + l2‬‬ ‫‪li‬‬ ‫‪L o , L c1 , L c2‬‬ ‫ המרחק בין נקודות הדרך ) ‪ ( xdi , ydi‬ו‪. ( xdi +1 , ydi +1 ) -‬‬‫‪ -‬סימון מערכות‪.‬‬ ‫‪P‬‬ ‫ מטריצה חיובית‪ ,‬משמשת בתכנון בקר המבוסס על שימוש‬‫במודל כלי רכב עם החלקות‪.‬‬ ‫)‪P(t‬‬ ‫ קווריאנס שגיאת השערוך‪ ,‬פתרון משוואת ריקאטי )משמש‬‫בתכנון מסנן קלמן(‪.‬‬ ‫) ‪q1 ( t ) , q 2 ( t‬‬ ‫ פונקציות המוגדרות ע"י מתכנן מערכת הבקרה וקובעות את‬‫הקשר בין מהירות כלי הרכב ובין הנקודה שבה נמדד המסלול‬ ‫הרצוי‪.‬‬ ‫‪q3 ( t ) , q5 ( t ) ,‬‬ ‫‪q 7 ( t ) , q9 ( t ) ,‬‬ ‫) ‪q11 ( t‬‬ ‫‪Q‬‬ ‫)‪r(t‬‬ ‫‪R‬‬ ‫‪R‬‬ ‫ פונקציות המהוות מרכיב בהגדרה של‪ q1 ( t ) :‬ו‪) q 2 ( t ) -‬נגזרות‬‫מתוך התכונות שצריכות לקיים הפונקציות ) ‪ q1 ( t‬ו‪.( q 2 ( t ) -‬‬ ‫ מטריצה הופכית של ‪.P‬‬‫ רדיוס המגדיר את מיקום כלי הרכב במערכת צירים פולארית‪.‬‬‫ מטריצת רוטציה ) ‪.( p = RP‬‬‫קווריאנס רעש המדידה )משמש בתכנון מסנן קלמן(‪.‬‬ ‫ו‬ ‫) ‪s (t‬‬ ‫‪ -‬קיצור‪ ,‬מוגדר ע"י ) ) ‪. s ( t ) = sin ( θ ( t‬‬ ‫)‪v(t‬‬ ‫‪ -‬המהירות הקווית של כלי הרכב‪.‬‬ ‫)‪v(t‬‬ ‫‪ -‬אות נתון‪ ,‬הקשור במהירות של כלי הרכב‪.‬‬ ‫) ‪vx ( t‬‬ ‫ רכיב מהירות קווית של כלי רכב בכיוון ציר ‪) x‬ביחס למערכת‬‫צירים הרתומה לכלי הרכב(‪.‬‬ ‫) ‪vy ( t‬‬ ‫ רכיב מהירות קווית של כלי רכב בכיוון ציר ‪) y‬ביחס למערכת‬‫צירים הרתומה לכלי הרכב(‪.‬‬ ‫) ‪vw ( t‬‬ ‫‪ -‬מהירות קווית של גלגל‪.‬‬ ‫)‪w(t‬‬ ‫ מהירות סיבוב הגלגל הקדמי )מהירות סיבוב ההגה( במודל מסוג‬‫‪.bicycle‬‬ ‫)‪( x, y‬‬ ‫‪ -‬מערכת צירים מקומית )רתומה לכלי הרכב(‪.‬‬ ‫) ‪( x r , yr‬‬ ‫‪ -‬מערכת צירים מקומית ) "צמודה למסלול" בנקודה ‪.(s‬‬ ‫) ‪( X, Y‬‬ ‫‪ -‬מערכת צירים אינרציאלית )קבועה במרחב(‪.‬‬ ‫‪yd‬‬ ‫ קואורדינאטה ‪ y‬של נקודת יחוס על המסלול הרצוי )מוגדרת‬‫עבור בקר המיועד ל‪.(trajectory tracking -‬‬ ‫) ‪yf ( t‬‬ ‫‪ -‬מיקום כלי הרכב‪ ,‬קואורדינאטה ‪ y‬של מרכז הסרן הקדמי בכלי‬ ‫) ‪αw ( t‬‬ ‫רכב בעל ארבעה גלגלים‪.‬‬ ‫‪ -‬זווית החלקה של גלגל‪.‬‬ ‫) ‪α1 ( t‬‬ ‫‪ -‬זווית החלקה של הגלגל האחורי במודל ‪.bicycle‬‬ ‫) ‪α2 ( t‬‬ ‫‪ -‬זווית החלקה של הגלגל הקדמי במודל ‪.bicycle‬‬ ‫) ‪θf ( t‬‬ ‫ כיוון הגלגל הקדמי במודל מסוג ‪) bicycle‬ביחס למערכת צירים‬‫) ‪ ( X, Y‬קבועה במרחב(‪.‬‬ ‫)‪ν(t‬‬ ‫‪ -‬המהירות הקווית של הגלגל הקדמי במודל מסוג ‪.bicycle‬‬ ‫)‪ν(t‬‬ ‫‪ -‬ווקטור רעשי מדידה‪.‬‬ ‫) ‪χ(t‬‬ ‫‪ -‬ווקטור משתני מצב של מודל כלי רכב‪.‬‬ ‫ז‬ ‫‪ -1‬הקדמה‬ ‫‪ – 1.1‬רקע‬ ‫טכנולוגיה מתקדמת המאפשרת עיבוד מהיר ויעיל של מידע והרצון להפחית‬ ‫למינימום האפשרי השקעה של משאב אנושי במשימות הניתנות לביצוע ע"י מכונה‬ ‫מהווים תמריץ לפיתוח מכונות אוטונומיות אשר מסוגלות לקבל החלטות ולבצע‬ ‫פעולות ללא התערבות אנושית‪ .‬משימות רבות המיועדות לביצוע ע"י מכונות‬ ‫אוטונומיות דורשות יכולת ניידות ולכן מצריכות פיתוח של כלי רכב אוטונומיים‪.‬‬ ‫מס' דוגמאות לשימושים בכלי רכב אוטונומיים‪:‬‬ ‫• תנועה באזורים בהם קיימת סכנה לחיי אדם‪ ,‬הן ביישומים צבאיים לדוגמה‬ ‫שדה מוקשים‪ ,‬שטח אויב וכו'‪ ,‬והן ביישומים אזרחיים כמו למשל תנועה‬ ‫מכרות‪.‬‬ ‫•‬ ‫נהיגה של מכוניות בכבישים מהירים בהן מתרחשות תאונות קטלניות‬ ‫כתוצאה מטעויות אנוש‪.‬‬ ‫• הפחתת עלויות יצור בתעשייה ע"י שילוב כלי רכב אוטונומיים‪ ,‬כמו למשל‬ ‫שינוע חלקים ממקום למקום‪.‬‬ ‫יכולת הניידות האוטונומית מחייבת שימוש באלגוריתמים אשר יבטיחו נסיעה של‬ ‫כלי הרכב לאורך מסלול רצוי ותיקון שגיאות הנגרמות כתוצאה מאי וודאות‬ ‫הקשורה במצב הדינאמי והקינמטי של כלי הרכב‪.‬‬ ‫במסגרת תכנון בקרים לכלי רכב אוטונומיים נהוג להבחין בשתי בעיות שונות‪:‬‬ ‫‪ .1‬נסיעה לאורך עקום )‪ – (path following‬כלי הרכב נדרש לנסוע לאורך עקום‬ ‫המוגדר באופן גיאומטרי‪ ,‬ללא התייחסות לזמן‪.‬‬ ‫‪ .2‬תנועה במסלול מוגדר )‪ – (trajectory tracking‬כלי הרכב נדרש לנוע במסלול‬ ‫המוגדר באופן גיאומטרי וכולל גם התייחסות לזמן )כלומר כלי הרכב צריך‬ ‫להיות במקום מסוים בזמן מסוים(‪.‬‬ ‫קיימת גם בעיה שלישית הנקראת יציבות סביב נקודה )‪ (set point‬ובה כלי הרכב‬ ‫נדרש להגיעה למצב רצוי קבוע בזמן‪ ,‬בעיית היציבות סביב נקודה אינה נדונה‬ ‫בעבודה זו‪ .‬בכל המקרים מניחים כי מרחב העבודה וכמובן מסלול הנסיעה הרצוי‬ ‫נקיים ממכשולים‪ .‬מבחינה מעשית‪ ,‬כלי רכב אוטונומיים כוללים גם מערכות‬ ‫המאפשרות זיהוי מכשולים‪ ,‬בדרך כלל על ידי שימוש במצלמות ווידאו או סורקי‬ ‫לייזר‪ .‬האלגוריתמים המפותחים במסגרת עבודה זו מיועדים למקרה בו קיימת‬ ‫אינפורמציה מספיקה המאפשרת תכנון מסלול נקי ממכשולים‪.‬‬ ‫‪1‬‬ ‫המטרה של עבודת מחקר זו היא פיתוח אלגוריתמים חדשים וברי מימוש המיועדים‬ ‫לפתרון בעיית ה‪ .path following -‬מלבד הגישה התיאורטית המוצגת בתכנון מערכות‬ ‫בקרה‪ ,‬נדונים גם היבטים מעשיים הקשורים במימוש‪ ,‬כגון‪ :‬תכנון מסלול‪ ,‬שינוי‬ ‫מערכות צירים בזמן אמת ושימוש באינפורמציה המתקבלת מחיישנים‪ .‬באופן‬ ‫מצומצם נדונים גם נושאים הקשורים בהרחבת השיטות ופתרון בעיית ה‪trajectory -‬‬ ‫‪.tracking‬‬ ‫עבודת המחקר עוסקת בתכנון מערכות בקרה לשני סוגים של כלי רכב )בגלל הגישה‬ ‫הכללית‪ ,‬כפי שיובהר בהמשך‪ ,‬מתאימה גם לסוגים נוספים(‪ .‬הסוג הראשון הינו כלי‬ ‫רכב בעל הנעה דיפרנציאלית כפי שמופיע באיור ‪ .1.1‬לכלי רכב זה שני גלגלים‬ ‫מונעים המותקנים לאורך ציר משותף )וגלגלים נוספים המשמשים לצורך יציבות‬ ‫כלי הרכב בלבד(‪ .‬סיבוב כלי הרכב מתאפשר ע"י הפעלת כל אחד משני הגלגלים‬ ‫המונעים במהירות סיבובית שונה‪ .‬הנעה דיפרנציאלית מאפשרת יכולת תמרון‬ ‫גבוהה והיא נפוצה בכלי רכב חשמליים קטנים כגון רובוטים ניידים‪.‬‬ ‫‪θ‬‬ ‫‪y‬‬ ‫‪x‬‬ ‫איור ‪ – 1.1‬שרטוט סכמאטי של כלי רכב בעל הנעה דיפרנציאלית‬ ‫גישה נפוצה המשמשת לתכנון מערכות בקרה לכלי רכב בעלי הנעה דיפרנציאלית‬ ‫מבוססת על שימוש במודל קינמטי מסוג ‪) unicycle‬חד אופן(‪ .‬במקרה זה המודל‬ ‫הקינמטי מתאר תנועה של גלגל אחד הנמצא במרכז הסרן המחבר את שני הגלגלים‬ ‫המניעים וכיוונו זהה לכיוון כלי הרכב‪ .‬מודל זה מתואר ע"י המשוואה הבאה‪:‬‬ ‫)‪(1.1‬‬ ‫‪x& = cos θ v‬‬ ‫‪y& = sin θ v‬‬ ‫‪θ& = ω‬‬ ‫כאשר הנקודה ) ‪ ( x , y‬היא מרכז הציר המחבר את שני הגלגלים המניעים‪ ,‬הזווית ‪θ‬‬ ‫היא הזווית בין כלי הרכב וציר ‪ - v , x‬מהירות קווית ו‪ ω -‬מהירות זוויתית‪ .‬לצורך‬ ‫בקרת התנועה של כלי הרכב משמשות המהירויות ‪ v‬ו‪ ω -‬כאותות בקרה )מוצא‬ ‫הבקר(‪.‬‬ ‫‪2‬‬ ‫כלי רכב נוסף הנדון בעבודה זו בהרחבה הינו כלי רכב בעל ארבעה גלגלים עם היגוי‬ ‫קדמי והנעה אחורית‪ ,‬כפי שמופיע באיור ‪ .1.2‬לכלי רכב זה מנוע אחד המפעיל את‬ ‫שני הגלגלים האחוריים באמצעות דיפרנציאל ופעולת ההיגוי מבוצעת ע"י הטיית‬ ‫הגלגלים הקדמיים‪ .‬מבנה זה משמש בכלי רכב כגון מכוניות‪ ,‬משאיות וטרקטורים‪.‬‬ ‫‪φ‬‬ ‫‪L‬‬ ‫‪θ‬‬ ‫‪yf‬‬ ‫‪y‬‬ ‫‪x‬‬ ‫‪xf‬‬ ‫איור ‪ – 1.2‬שרטוט סכמאתי של כלי רכב בעל ארבעה גלגלים‬ ‫מודל מקובל המשמש לתכנון מערכות בקרה לכלי רכב כגון זה המופיע באיור ‪1.2‬‬ ‫הוא מודל קינמטי מסוג ‪ ,bicycle‬מודל כזה נתון במשוואה הבאה‪:‬‬ ‫‪x& = cos θ v‬‬ ‫‪y& = sin θ v‬‬ ‫)‪(1.2‬‬ ‫‪tan φ‬‬ ‫‪v‬‬ ‫= &‪θ‬‬ ‫‪L‬‬ ‫‪φ& = w‬‬ ‫מודל קינמטי מסוג ‪ bicycle‬מבוסס על מבנה הכולל שני גלגלים‪ .‬הראשון נמצא‬ ‫בנקודה ) ‪) ( x , y‬כלומר מרכז הסרן האחורי( ומכוון בזווית ‪ θ‬ביחס לציר ‪ ,x‬גלגל‬ ‫נוסף נמצא במרכז הסרן הקדמי ומכוון בזווית ‪ φ‬ביחס למסגרת כלי הרכב‪ .‬כדי‬ ‫להפחית ככל האפשר החלקה של הגלגלים הקדמיים בכלי הרכב הנתון באיור ‪,1.2‬‬ ‫הגלגלים הקדמיים אינם מכוונים בזוויות שוות ביחס למסגרת כלי הרכב‪ ,‬הזווית ‪φ‬‬ ‫במודל )‪ (1.2‬היא הזווית שיש לכוון גלגל יחיד הנמצא במרכז הסרן הקדמי כך‬ ‫שיתקבל רדיוס סיבוב השווה לזה המתקבל בכלי רכב בעל ארבעה גלגלים )זווית‬ ‫זאת נקראת גם זווית אקרמן(‪.‬‬ ‫אותות הבקרה במודל )‪ (1.2‬הם‪ :‬המהירות הקווית של כלי הרכב המסומנת ע"י ‪v‬‬ ‫)המהירות הקווית של הנקודה ) ‪ ( ( x , y‬ומהירות סיבוב הגלגל הקדמי המסומנת ע"י‬ ‫‪) w‬מהירות סיבוב ההגה(‪.‬‬ ‫‪3‬‬ ‫שימוש במהירות זוויתית ) &‪ ( w = φ‬כאות בקרה עדיף על שימוש בזווית ) ‪ ( φ‬כאות‬ ‫בקרה משני טעמים‪ .‬ראשית‪ ,‬אי רציפות באות הבקרה אינה גוררת אי רציפות‬ ‫בזווית הגלגלים הקדמיים ושנית וויסות של מהירות זוויתית במנוע חשמלי )המפעיל‬ ‫את ההגה‪ ,‬במקרה זה( פשוטה יותר למימוש בהשוואה לוויסות של זווית ] ‪Samson‬‬ ‫‪.[1993‬‬ ‫למודל )‪ (1.2‬מצב סינגולארי כאשר ‪ , φ = ± π 2‬אולם מבחינה מעשית בכלי רכב בעל‬ ‫ארבעה גלגלים קיימים אילוצים מכאניים אשר אינם מאפשרים סיבוב ההגה בזוויות‬ ‫קטנות בהרבה מ‪ . φ = ± π 2 -‬עובדה זו מצריכה התייחסות מתאימה בתכנון מערכת‬ ‫הבקרה‪ .‬המצב הסינגולארי ‪ φ = ± π 2‬אינו קיים במודל ‪ bicycle‬המתאים לכלי רכב‬ ‫בעל ארבעה גלגלים עם הנעה קדמית‪ .‬במקרה זה המודל הקינמטי נתון ע"י‪:‬‬ ‫‪x& = cos θ cos φ ν‬‬ ‫‪y& = sin θ cos φ ν‬‬ ‫‪sin φ‬‬ ‫= &‪θ‬‬ ‫‪ν‬‬ ‫‪L‬‬ ‫‪φ& = w‬‬ ‫)‪(1.3‬‬ ‫כאשר ) ‪ ν ( t‬המהירות הקווית של הגלגל הקדמי‪.‬‬ ‫למרות שכל העבודה בהמשך מתייחסת למקרה בו מדובר בכלי רכב בעל הנעה‬ ‫אחורית‪ .‬שימוש בקשר‪:‬‬ ‫)‪(1.4‬‬ ‫‪v = cos φ ν‬‬ ‫מאפשר מימוש האלגוריתמים גם בכלי רכב בעל הנעה קדמית )במקרה של מימוש‬ ‫בכלי רכב בעל הנעה קדמית על ידי שימוש בטרנספורמציה )‪ (1.4‬יש לבדוק את‬ ‫השפעתה על המצבים הסינגולאריים הקיימים בחוק הבקרה ועל התנאים הנחוצים‬ ‫ליציבות החוג הסגור(‪.‬‬ ‫כלי הרכב הנדונים בעבודה זו הינם בעלי אילוצים לא הולונומיים‪ ,‬אילוצים אלה‬ ‫נובעים משימוש בגלגלים המאפשרים תנועה בכיוונים מסוימים בלבד וקובעים‬ ‫אילוץ מהצורה הבאה על המהירויות של כלי הרכב‪:‬‬ ‫)‪(1.5‬‬ ‫‪C ( χ ) χ& = 0‬‬ ‫כאשר ‪ χ‬ווקטור משתני המצב של כלי הרכב‪ .‬במקרה שבו המודל מבוסס על הנחה‬ ‫של גלגול ללא החלקה )כמו ב‪ (1.1) -‬ו‪ ((1.2) -‬האילוץ המוכתב ע"י כל אחד מגלגלי‬ ‫כלי הרכב קובע מהירות אפס בניצב לגלגל‪ .‬עבור מודל ה‪:(1.1) unicycle -‬‬ ‫‪4‬‬ ‫]‪y θ‬‬ ‫‪T‬‬ ‫‪x& sin θ − y& cos θ = 0 , χ = [ x‬‬ ‫⇒‬ ‫)‪(1.6‬‬ ‫]‪C ( χ ) = [sin θ − cos θ 0‬‬ ‫ועבור מודל ה‪) (1.2) bicycle -‬כאן קיימם שני אילוצים‪ ,‬אחד עבור הגלגל האחורי‬ ‫ושני עבור הגלגל הקדמי(‪:‬‬ ‫]‪y θ φ‬‬ ‫‪T‬‬ ‫‪x& sin θ − y& cos θ = 0 , χ = [ x‬‬ ‫‪x& f sin ( θ + φ ) − y& f cos ( θ + φ ) = 0 , x f = x + L cos θ , y f = y + L sin θ‬‬ ‫)‪(1.7‬‬ ‫⇒‬ ‫‪− cos θ‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪ sin θ‬‬ ‫‪C(χ) = ‬‬ ‫‪‬‬ ‫‪sin ( θ + φ ) − cos ( θ + φ ) − L cos ( φ ) 0 ‬‬ ‫ניתן לחלק את מערכות הבקרה הנדונות בעבודה לשתי קבוצות באופן הבא‪,‬‬ ‫מערכות בקרה הפועלות ביחס למערכת קואורדינאטות קבועה במרחב )מערכת‬ ‫אינרציאלית( ומערכות בקרה הפועלות ביחס למערכת קואורדינאטות מקומית‬ ‫)מערכת שאינה קבועה במרחב(‪ .‬במקרה הראשון כלי הרכב נדרש לעקוב אחרי‬ ‫מסלול הנתון במערכת קואורדינאטות קבועה במרחב‪ ,‬ואות הבקרה מבוסס על‬ ‫מדידה של מצב כלי הרכב ביחס למערכת זו‪ .‬חיישנים נפוצים במקרה זה הם ‪,GPS‬‬ ‫‪ INS ,DGPS‬ומצפן מגנטי‪ .‬במקרה השני מצב כלי הרכב ביחס למסלול הרצוי נמדד‬ ‫במערכת קואורדינאטות שאינה קבועה במרחב‪ .‬שתי מערכות קואורדינאטות‬ ‫מקומיות אפשריות במקרה זה‪ ,‬אחת רתומה לכלי הרכב והשנייה צמודה למסלול‬ ‫הרצוי‪ .‬חיישנים נפוצים במקרה זה הם מצלמת ווידאו וסורק לייזר אשר מותקנים‬ ‫על כלי הרכב‪ ,‬מדידת המצב של כלי הרכב במרחב )לדוגמא מדידת ‪ (GPS‬אינה‬ ‫נחוצה במקרה זה‪.‬‬ ‫היעד של האלגוריתמים המפותחים בעבודה הוא כלי רכב אוטונומיים המופעלים‬ ‫בתנאי שדה )‪ ,(outdoor‬בדרך כלל כלים אלה הינם מסוג כלי רכב בעל ארבעה‬ ‫גלגלים‪ .‬מסיבה זו‪ ,‬מלבד פרק מספר ‪ 2‬אשר דן בשני סוגי כלי הרכב‪ ,‬עיקר העבודה‬ ‫עוסק באופן ישיר בפתרון בעיית ה‪ path following -‬בכלי רכב בעל ארבעה גלגלים‪.‬‬ ‫כדי לבחון ולעורר סוגיות שונות הקשורות במימוש מערכת הבקרה נבנה )במסגרת‬ ‫עבודת המחקר( כלי רכב חשמלי בעל ארבעה גלגלים‪ .‬כלי רכב זה שימש לביצוע‬ ‫ניסויים בתנאי שדה )‪ (outdoor‬של חלק מהנושאים התיאורטיים הנדונים בעבודה‪,‬‬ ‫תוצאות ומסקנות מתוך ניסויים אלה מוצגים בהמשך‪.‬‬ ‫‪5‬‬ ‫‪ – 1.2‬סקר ספרות‬ ‫ניתן למצוא בספרות המדעית שפע של עבודות העוסקות בפתרון בעיות ה‪path -‬‬ ‫‪ following‬וה‪ trajectory tracking -‬ומבוססות על שימוש במודלים קינמאטיים‪ .‬בדרך‬ ‫כלל האלגוריתמים המיועדים לפתרון בעיית ה‪ trajectory tracking -‬הם אלגוריתמים‬ ‫ביחס למערכת קואורדינאטות קבועה במרחב המבוססים על מדידת השגיאה בין‬ ‫כלי הרכב ובין רכב ייחוס‪ .‬רכב ייחוס הוא רכב הנע על המסלול הרצוי ומקיים את‬ ‫אילוצי הזמן‪ ,‬מודל של רכב יחוס בעל ארבע גלגלים נתון ע"י‪:‬‬ ‫∗‪x& ∗ = cos θ∗ v‬‬ ‫∗‪y& ∗ = sin θ∗ v‬‬ ‫)‪(1.8‬‬ ‫∗ ∗‪tan φ‬‬ ‫= ∗ &‪θ‬‬ ‫‪v‬‬ ‫‪L‬‬ ‫∗ ‪φ& ∗ = w‬‬ ‫הבקר בשיטה זו מתוכנן על בסיס מודל המתקבל על ידי הגדרת שגיאות באופן הבא‪:‬‬ ‫‪e1 = x& ∗ − x‬‬ ‫)‪(1.9‬‬ ‫‪e2 = y& ∗ − y‬‬ ‫‪e = θ& ∗ − θ‬‬ ‫‪3‬‬ ‫‪e4 = φ& ∗ − φ‬‬ ‫על פי עקרון זה מוצע ב‪ [Jiang & Nijmeijer 1997] -‬בקר לכלי רכב בעל הנעה‬ ‫דיפרנציאלית כאשר המחברים מוכיחים יציבות חצי גלובלית או גלובלית עבור‬ ‫עקומים מסוימים )כגון קוו ישר או מעגל( ע"י שימוש ישיר בשיטת ליאפונוב‪.‬‬ ‫המאמר כולל גם הרחבה למודל דינאמי מופשט ע"י שימוש בשיטה הנקראת ‪back-‬‬ ‫‪ .stepping‬ב‪ [Ailon et al. 2000] -‬מוצעים אלגוריתמים המיועדים לבקרה של כלי רכב‬ ‫בעל הנעה דיפרנציאלית וכלי רכב בעל ארבעה גלגלים בגישה המבוססת על עקיבה‬ ‫אחרי רכב ייחוס‪ .‬במאמר גם נדון נושא הקונטרולביליות ומוצע לשימוש‪ ,‬כאשר‬ ‫השגיאה ההתחלתית גדולה‪ ,‬פעולה בשני שלבים‪ ,‬בשלב הראשון מופעל אות בקרה‬ ‫בחוג פתוח אשר מאפשר תנועה של כלי הרכב מכל תנאי התחלה אל כל מצב סופי‬ ‫רצוי‪ ,‬אות זה מאפשר להביא את כלי הרכב קרוב למסלול הרצוי‪ ,‬בשלב השני מופעל‬ ‫אות בקרה בחוג סגור המאפשר יציבות מקומית‪ .‬דוגמאות נוספות לתכנון המבוסס‬ ‫על רכב ייחוס ניתן למצוא ב‪ [Laugier et al. 1998] -‬שם הבקר מתאים לכלי רכב בעל‬ ‫ארבעה גלגלים וב‪ [Dixon et al. 2000] -‬עבור כלי רכב בעל הנעה דיפרנציאלית‪.‬‬ ‫ב‪ [Gusev et al. 1998] -‬מוצג פיתוח של בקר לכלי רכב בעל ארבעה גלגלים כאשר‬ ‫המטרה היא נהיגה אוטונומית במסלול המוגדר באופן פרמטרי וכלי הרכב נדרש‬ ‫‪6‬‬ ‫לנסוע במהירות רצויה‪ .‬אות הבקרה במקרה זה )זווית ההגה ‪ , φ‬ולא &‪ φ‬כמו ב‪((1.3) -‬‬ ‫מפותח על בסיס מודל קינמאטי‪ .‬במאמר נעשה גם שימוש במודל דינאמי מופשט‬ ‫ושיטות אדפטיביות כדי להבטיח הפעלה של כלי הרכב במהירות רצויה וסיבוב‬ ‫ההגה בזווית מתאימה כפי שחושבה על בסיס המודל הקינמטי‪ .‬ב‪[Kim & Oh 1999] -‬‬ ‫מוצע בקר לכלי רכב בעל הנעה דיפרנציאלית ביחס למערכת קואורדינאטות קבועה‬ ‫במרחב המבוסס על "‪ "input-output linearization‬כאשר מצב כלי הרכב נמדד ע"י‬ ‫מצלמה המותקנת מעל מרחב התנועה ומודדת מקום של נקודה אחת )במישור ‪(x,y‬‬ ‫הרתומה לכלי הרכב )על ציר הסימטריה אך לא במרכז הסרן המחבר את שני‬ ‫הגלגלים המניעים(‪.‬‬ ‫ב‪ [Consolini at al. 2001] -‬מוצע בקר לנהיגה אוטונומית של כלי רכב בעל ארבעה‬ ‫גלגלים לאורך מסלול הנתון במערכת קואורדינאטות קבועה במרחב‪ .‬בעבודה זו‬ ‫)באופן קצת שונה מהמקובל( נקודה הנמצאת על ציר הסימטריה במרחק קבוע ‪d‬‬ ‫לפני כלי הרכב נדרשת לעקוב אחרי המסלול הרצוי‪ .‬הפתרון במאמר מבוסס על אות‬ ‫בקרה בחוג פתוח אשר תלוי בתנאי ההתחלה של כלי הרכב‪ .‬המחבר גם מציין‬ ‫תנאים מספקים הקשורים בתכונות הגיאומטריות של העקום‪ ,‬בתנאי ההתחלה‬ ‫ובמרחק הקבוע ‪ d‬כך שהבעיה תהיה ברת פתרון‪.‬‬ ‫אלגוריתמים המבוססים על שימוש במערכת קואורדינאטות קבועה במרחב‬ ‫מצריכים מדידה של מצב כלי הרכב ביחס למערכת זו‪ .‬רוב החיישנים המיועדים‬ ‫לספק אינפורמציה כזאת סובלים מבעיות דיוק כגון סחיפה במערכות אינרציאליות‪,‬‬ ‫החלקה של גלגלים במערכות אודומטריות המבוססות על אנקודרים ודיוק לא‬ ‫מספק של מערכות ‪ .GPS‬מחירם של מערכות חישה אינרציאליות ומערכות ‪DGPS‬‬ ‫המתאימים לשימוש בכלי רכב אוטונומיים עשוי להיות גבוהה מאוד‪ .‬מצד שני‪,‬‬ ‫עלויות הולכות וקטנות של אמצעים לראיה מלאכותית ויכולות חישוב מתקדמות‬ ‫מהוות תמריץ לשילוב אינפורמציה חזותית במשוב מערכת הבקרה‪ .‬כאשר המטרה‬ ‫היא ‪ path following‬וכאשר מסלול הנסיעה הרצוי ניתן למדידה במערכת‬ ‫קואורדינאטות רתומה לכלי הרכב )לדוגמה ע"י שימוש במצלמה( בעיית העקיבה‬ ‫ניתנת לפתרון ללא צורך במדידת מצב כלי הרכב במרחב‪ ,‬פתרון זה מבוסס על‬ ‫מערכת בקרה בקואורדינאטות מקומיות )כפי שהוגדרה בעבודה זו(‪.‬‬ ‫ב‪ [Frezza at al.1998] -‬מוצע בקר המבוסס על מדידת המסלול במערכת‬ ‫קואורדינאטות רתומה לכלי רכב בעל הנעה דיפרנציאלית‪ ,‬חישוב עקום פלינומיאלי‬ ‫מהנקודה שבה נמצא כלי הרכב אל נקודה הנמצאת על המסלול הרצוי והפעלת אות‬ ‫בקרה בחוג פתוח המאפשר נסיעה על העקום המתוכנן‪ .‬בגלל תנועת כלי הרכב‬ ‫ושינוי נקודת החיבור אל המסלול הרצוי גם עקום החיבור משתנה בזמן‪ .‬במאמר‬ ‫‪7‬‬ ‫מוכחת יציבות מקומית ע"י שימוש בלינאריזציה של מודל כלי הרכב והבקר‪ .‬ב‪Ma ] -‬‬ ‫‪ [at al. 1999‬מתוכנן בקר אשר מאפשר נהיגה אוטונומית של רובוט נייד )בעל הנעה‬ ‫דיפרנציאלית( במסלול אשר העקמומיות בו משתנה באופן ליניארי‪ .‬שגיאת העקיבה‬ ‫במקרה זה‪ ,‬נמדדת במערכת קואורדינאטות של מצלמה המותקנת על כלי הרכב‬ ‫ונעשה שימוש בתיאורית היציבות של ליאפונוב להוכחת יציבות מקומית‪ .‬גישה‬ ‫אחרת למערכת בקרה בקואורדינאטות מקומיות ניתן למצוא ב‪[Aguilar at al. 1988] -‬‬ ‫שם מתוכנן בקר לרובוט נייד בעל הנעה דיפרנציאלית כאשר שגיאות העקיבה‬ ‫נמדדות במערכת קואורדינאטות המשיקה למסלול בנקודה אשר משתנה עם תנועת‬ ‫כלי הרכב )מערכת מסלול(‪ .‬בקר זה מבטיח התכנסות אקספוננציאלית‪ ,‬וכן‬ ‫רובסטיות ביחס לרעשי מדידה‪.‬‬ ‫גישה אחרת אשר אינה מצריכה מודל אנליטי של התהליך נתונה ב‪Garnier & ] -‬‬ ‫‪ [Fraichard 1996‬ו‪ [Hajjaji & Bentalba 2003] -‬שם נעשה שימוש בבקר מסוג ‪Fuzzy‬‬ ‫‪ Logic‬כדי לאפשר נסיעה של כלי רכב בעל ארבעה גלגלים לאורך מסלול רצוי‪.‬‬ ‫המשותף לכל האלגוריתמים האחרונים היא העובדה שהם פותחו למקרה פרטי בו‬ ‫מעוניינים לבקר כלי רכב בעל הנעה דיפרנציאלית או כלי רכב בעל ארבעה גלגלים‬ ‫)כלומר הבקר מתאים לכלי רכב מסוים(‪ .‬גישה אחרת בעלת יתרונות ברורים‬ ‫מבוססת על פיתוח אלגוריתמים המתאימים לקבוצה גדולה של מערכות הכוללת‬ ‫בתוכה גם כלי רכב‪ .‬מערכות מכאניות רבות הנן מערכות בעלות אילוצים לא‬ ‫הולונומיים‪ ,‬חלק ממערכות אלה )בכללן כלי רכב( ניתן לייצג ע"י שימוש במודל‬ ‫הנקרא "מודל שרשרת"‪ .‬תנאים מספיקים הקשורים בייצוג מערכת בעלת אילוצים‬ ‫לא הולונומיים על ידי מודל בצורת שרשרת ניתן למצוא ב‪.[Tilbury at al. 1995] -‬‬ ‫עבודה רבה נעשתה במהלך השנים סביב מודל שרשרת בהקשר של בקרת כלי רכב‬ ‫והוצעו בקרים רבים‪ ,‬כמעט כולם מיועדים לפתרון בעיית היציבות סביב נקודה‪,‬‬ ‫ראה לדוגמא ]‪Sordalen & Egeland ] ,[Laiou & Astolfi 1999] ,[Tayebi et al. 1997‬‬ ‫‪ .[1995‬הסיבה לעניין הגדול שמעוררת בעיית היציבות סביב נקודה בהקשר זה‬ ‫קשורה בעובדה שבעיה זו קשה יותר לפתרון ביחס לבעיית העקיבה‪ .‬בניגוד לבעיית‬ ‫ה‪ path following -‬ולמרות העובדה שהמודלים הקינמאטיים )‪ (1.1‬ו‪ (1.2) -‬הינם‬ ‫קונטרולביליים )]‪ ([De Luca et al. 1998] ,[Ailon et al. 2001‬בעיית היציבות סביב‬ ‫נקודה בכלי הרכב הנדונים אינה ניתנת לפתרון ע"י בקר משוב מצב חלק וקבוע בזמן‬ ‫)]‪.(Brockett's theorem [Brockett 1983‬‬ ‫למרות הפשטות והאטרקטיביות הגדולה של שימוש במודל שרשרת לטיפול בבעיית‬ ‫ה‪ path following -‬לא ניתן למצוא עבודות רבות בנושא זה‪ .‬ב‪ [Samson 1995] -‬נעשה‬ ‫שימוש בהמרת נגזרות בזמן בנגזרות ביחס לציר ‪ ,x‬המרה זו אפשרה תכנון בקר‬ ‫‪8‬‬ ‫משוב מצב‪ ,‬על בסיס מודל ליניארי קבוע בזמן‪ ,‬ושימושו בבקרה של כלי רכב ביחס‬ ‫למערכת קואורדינאטות קבועה במרחב‪ .‬ב‪ [Lu et al. 2000] -‬מוצע בקר כללי‬ ‫למערכות הניתנות לייצוג בצורת שרשרת‪ ,‬המבוסס על שיטת ה‪ .sliding mode -‬גם‬ ‫שם מוצג מימוש רק עבור מודל שגיאות ביחס למערכת קואורדינאטות קבועה‬ ‫במרחב‪ .‬גישה אחרת )שאינה עושה שימוש בצורת שרשרת( לפיה מפותח בקר כללי‬ ‫המתאים לשימוש בכלי רכב אוטונומיים ניתן למצוא ב‪ ,[Levant 2001] -‬שם מוצע‬ ‫בקר בשיטת ה‪ sliding mode -‬המבוסס על הדרגה היחסית )‪ (relative degree‬של‬ ‫המערכת ומודגם שם כיצד ניתן לעשות בו שימוש כדי לפתור את בעיית ה‪path -‬‬ ‫‪ following‬בגישה של מערכת קואורדינאטות קבועה במרחב עבור כלי רכב בעל‬ ‫ארבעה גלגלים‪.‬‬ ‫בעבודת מחקר זו מוצע ע"י המחבר בקר חדש לייצוב מערכות אשר ניתנות לייצוג‬ ‫ע"י מודל שרשרת‪ .‬בקר זה שונה מהבקר המוצע ב‪ [Samson 1995] -‬מכיוון שהבקר‬ ‫המוצע כאן אינו ליניארי וכאשר הוא מיושם לצורך פתרון בעיית ה‪path following -‬‬ ‫בכלי רכב‪ ,‬הוא יכול למנוע )ע"י שימוש בפונקציה לא ליניארית מתאימה( את הצורך‬ ‫באותות בקרה גדולים )בהשוואה לבקר הליניארי ב‪ ([Samson 1995] -‬כאשר כלי‬ ‫הרכב נמצא רחוק מהמסלול הרצוי‪ .‬יתרונו של הבקר המוצע כאן על פני זה המוצע‬ ‫ב‪ [Lu et al. 2000] -‬ומבוסס על שיטת ה‪ sliding modes -‬הוא בהיותו חלק‪ ,‬ולכן אינו‬ ‫סובל מתופעת ‪ chattering‬כמו זה האחרון‪ .‬בנוסף לפיתוח הבקר בעבודה זו )פרק ‪(2‬‬ ‫מוצג כיצד ניתן לעשות בו שימוש לצורך פתרון בעיית ה‪ path following -‬במגוון גדול‬ ‫וייחודי של אפשרויות הנובע מהגדרות שונות של שגיאות כלי הרכב ביחס למסלול‬ ‫הרצוי ומפיתוח מודל השגיאות ביחס למערכות קואורדינאטות קבועות במרחב או‬ ‫מקומיות‪ .‬הדוגמא הראשונה בפרק ‪ 2‬מציגה מימוש של הבקר )אשר פותח בעבודה‬ ‫זו( כאשר מודל השרשרת מתקבל ע"י שימוש בטרנספורמציה הנפוצה עבור מערכת‬ ‫קואורדינאטות קרטזית קבועה במרחב‪ .‬לטרנספורמציה זו מצב סינגולארי כאשר‬ ‫כלי הרכב ניצב לציר ‪ x‬של המערכת הקרטזית ודוגמאות לשימוש בה ניתן למצוא ב‪-‬‬ ‫]‪ [Lu et al. 2000] ,[Samson 1995‬ו‪ .[Tayebi et al. 2001] -‬כדי להתגבר על החיסרון‬ ‫הקשור במצב הסינגולארי מוצעת בעבודה זו טרנספורמציה אחרת )פרק ‪(2‬‬ ‫ויתרונותיה ביחס לטרנספורמציה הראשונה נדונים בהרחבה‪.‬‬ ‫רעיון ייחודי )המוצג בפרק ‪ (2‬הוא שימוש בטרנספורמציה )למודל שרשרת(‬ ‫המבוססת על מודל פולארי של כלי רכב בעל ארבעה גלגלים‪ .‬מלבד העובדה‬ ‫שמערכת קואורדינאטות פולארית מאפשרת ייצוג נוח של מסלולים אשר אינם‬ ‫ניתנים לייצוג ע"י פונקציה אחת במערכת קואורדינאטות קרטזית יש לה גם‬ ‫יתרונות מסוימים הקשורים במימוש מערכת הבקרה ונדונים בפרק ‪ .4‬שימוש במודל‬ ‫‪9‬‬ ‫פולארי של כלי רכב לתכנון בקר )לא בגישה של מודל שרשרת( אינו ייחודי לעבודה‬ ‫זו‪ .‬ניתן למצוא מספר עבודות )לדוגמא ]‪ ([Chwa 2004] ,[Yang et al. 1998‬המבוססות‬ ‫על שימוש במודל פולארי של כלי רכב בעל הנעה דיפרנציאלית ותכנון בקר בשיטת‬ ‫‪ .sliding modes‬שימוש במודל פולארי של כלי רכב בעל ארבעה גלגלים אינו נפוץ‪,‬‬ ‫דוגמא אחת ניתן למצוא ב‪ [Indiveri 1999] -‬שם מוצע בקר לפתרון בעיית היציבות‬ ‫סביב נקודה ואות הבקרה הוא זווית הגלגלים הקדמיים‪.‬‬ ‫במקרה של מודלים המבוססים על מערכת קואורדינאטות מקומית אשר רתומה‬ ‫לכלי הרכב‪ ,‬פיתוח מודלים בצורת שרשרת )בספרות המדעית( הינו נדיר‪ .‬ב‪Ma at ] -‬‬ ‫‪ [al. 1999‬ניתן למצוא טרנספורמציה המאפשרת מעבר למודל שרשרת‬ ‫בקואורדינאטות מקומיות עבור כלי רכב בעל הנעה דיפרנציאלית )אם כי הוא אינו‬ ‫משמש שם לתכנון בקר( וגם נדונה האפשרות לפיתוח מודל שרשרת‬ ‫בקואורדינאטות מקומיות לכלי רכב בעל ארבעה גלגלים‪ .‬אפשרות זו מבוססת על‬ ‫תנאי מספיק הנתון ב‪ .[Mammar at al. 2001] -‬בשני המאמרים האחרונים‬ ‫הטרנספורמציה המתאימה אינה נתונה ולא נעשה שימוש במודל שרשרת לצורך‬ ‫תכנון בקר בקואורדינאטות מקומיות לכלי רכב בעל ארבעה גלגלים )כפי שנעשה‬ ‫בעבודה זו(‪ .‬בפרק ‪ 2‬מוצגת דוגמא של מימוש הבקר הכללי )אשר פותח בעבודה זו(‬ ‫כאשר הטרנספורמציה למודל שרשרת מבוססת על מערכת קואורדינאטות צמודה‬ ‫למסלול )מערכת מסלול(‪ .‬טרנספורמציה כזאת אינה ייחודית לעבודה זו ונדונה גם‬ ‫ב‪ [Samson 1995] -‬בהקשר של כלי רכב עם נגררים‪.‬‬ ‫חותם את פרק ‪ 2‬רעיון ייחודי אשר נותן מסגרת אחת לכל הטרנספורמציות השונות‬ ‫)למודל שרשרת( שהוצגו‪ ,‬במסגרת רעיון זה מתוכנן בקר אינווריאנטי אשר ניתן‬ ‫לגזור ממנו את כל שאר הבקרים כמקרים פרטיים‪.‬‬ ‫המטרה המרכזית של האלגוריתמים המוצגים בעבודה היא פתרון בעיית ה‪path -‬‬ ‫‪ ,following‬למרות זאת הגישה הננקטת מאפשרת הרחבה מיידית )ופשוטה( גם‬ ‫לפתרון בעיית ה‪) trajectory tracking -‬בגישה שאינה מבוססת על רכב ייחוס(‪.‬‬ ‫הפתרון בעבודה לבעיית ה‪ path following -‬כולל אותות בקרה אשר מפעילים את‬ ‫מערכת ההגה של כלי הרכב )בקר הגה( ומבוססים על מדידת מצב כלי הרכב‪,‬‬ ‫התכונות הגיאומטריות של המסלול הרצוי ומהירות כלי הרכב ‪ . v‬בשיטה זו ‪ v‬הינו‬ ‫משתנה בלתי תלוי ונדרשת מדידה שלו בלבד‪ .‬הרחבה של השיטה לפתרון בעיית ה‪-‬‬ ‫‪ trajectory tracking‬אפשרית ע"י הוספת בקר מהירות ‪ v‬אשר תפקידו לאפשר‬ ‫התקדמות של כלי הרכב על המסלול בקצב רצוי‪ ,‬מבנה זה מתואר באופן סכמאטי‬ ‫באיור ‪.2.3‬‬ ‫‪10‬‬ ‫‪v‬‬ ‫מהירות קווית‬ ‫‪v‬‬ ‫פקודת היגוי‬ ‫‪ ω‬או ‪w‬‬ ‫בקר‬ ‫הגה‬ ‫בקר‬ ‫מהירות‬ ‫זמן הגעה‬ ‫רצוי ליעד‬ ‫תוואי קרקע‬ ‫מכשולים‬ ‫מצב כלי הרכב כפי‬ ‫שנמדד ע"י חיישנים‬ ‫מסלול נסיעה רצוי‬ ‫איור ‪ - 2.3‬מבנה מערכת בקרה לכלי רכב אוטונומי‬ ‫השיקול המרכזי לבחירת מבנה כזה הינו האופי השונה של שתי הדרישות‪ .‬הדרישה‬ ‫להתקדמות בקצב רצוי מצריכה בדרך כלל רמת דיוק נמוכה ואף מאפשרת תיקון נוח‬ ‫של שגיאות לעומת דרישה קשיחה לנסיעה על המסלול הרצוי המצריכה רמת דיוק‬ ‫גבוהה שכן שגיאה בביצוע חלק זה של משימת הבקרה יכולה לגרום לירידה של כלי‬ ‫הרכב מהמסלול והתנגשות במכשולים‪ .‬גישה זו שונה מהגישה המקובלת לפתרון‬ ‫בעיית ה‪ trajectory tracking -‬אשר בה כלי הרכב נדרש לעקוב אחרי רכב ייחוס )‪,‬‬ ‫]‪Ailon et al. ] ,[Laugier et al. 1998] ,[Gusev et al. 1998] ,[Jiang & Nijmeijer 1997‬‬ ‫‪ .([2001‬בגישה המקובלת קיים צימוד בין הדרישה לנסיעה על המסלול הרצוי‬ ‫והדרישה להתקדמות בקצב רצוי )חוק הזמן(‪ .‬המסלול הרצוי נמדד במקום בו נמצא‬ ‫רכב הייחוס והתכנסות אל המסלול הרצוי מצריכה התכנסות אל רכב הייחוס‪ .‬כלים‬ ‫פשוטים המאפשרים הרחבה של הפתרון המוצע בעבודה לבעיית ה‪path following -‬‬ ‫ובכך מהווים פתרון לבעיות ה‪ trajectory tracking -‬נדונים בפרק מספר ‪.2‬‬ ‫כאשר מלבד נהיגה אוטונומית לאורך מסלול רצוי‪ ,‬נדונים גם נושאים הקשורים‬ ‫במניעת ההתהפכות של כלי הרכב‪ ,‬דרוש מבנה שונה מזה המוצג ב‪ ,2.3 -‬נושאים‬ ‫אלה אינם נדונים בעבודה זו‪.‬‬ ‫בכל העבודות שהוזכרו עד כה פיתוח המודל הקינמאטי של כלי הרכב התבסס על‬ ‫הנחה של גלגול ללא החלקה‪ .‬במקרה של כלי רכב בעל הנעה דיפרנציאלית‪ ,‬הנפוץ‬ ‫ברובוטים ניידים הנעים בתנאי פנים )‪ (indoor‬ובמהירויות נמוכות‪ ,‬הנחה זו‬ ‫מאפשרת ביצועים מספקים‪ .‬גם עבור כלי רכב בעל ארבעה גלגלים‪ ,‬כאשר האילוצים‬ ‫הלא הולונומיים הנתונים ב‪ (1.5) -‬מתקיימים באופן מקורב )בדרך כלל במהירות‬ ‫נמוכה(‪ ,‬ניתן להזניח את האינרציה של כלי הרכב ולתכנן בקר המבוסס על מודל‬ ‫קינמאטי כגון )‪ (1.1‬או )‪) (1.2‬ראה ]‪ .([Gusev et al. 1998] ,[Frezza et al. 1998‬עובדה זו‬ ‫אף מקבלת חיזוק מתוצאות הניסויים אשר בוצעו במסגרת עבודה זו‪.‬‬ ‫‪11‬‬ ‫בגלל המבנה המיוחד של מערכת ההגה בכלי רכב בעל ארבעה גלגלים‪ ,‬כלי רכב זה‬ ‫רגיש הרבה יותר מכלי רכב בעל הנעה דיפרנציאלית להחלקות של הגלגלים‬ ‫ולאפשרות של פגיעה בביצועים כאשר מופעל עם בקר שתוכנן בהנחה של גלגול‬ ‫ללא החלקה‪ .‬גישה שונה מזו המבוססת על שימוש במודל הקינמאטי )‪ (1.2‬לתכנון‬ ‫בקרים לכלי רכב )בעלי ארבעה גלגלים( הנעים בתנאי שדה )‪ ,(outdoor‬מבוססת על‬ ‫שימוש במודל דינאמי ליניארי )מסוג ‪ ,(bicycle‬מודל כזה אינו מבוסס על הנחה של‬ ‫גלגול ללא החלקה‪ .‬מספר דוגמאות לכך ניתן למצוא ב‪Dickmans & Mysliwetz ] -‬‬ ‫‪) [1992‬מהחלוצים בתחום זה( ו‪ .[Kosecka et al. 1998] -‬בשני המקרים מתוכנן בקר‬ ‫אשר מיועד להפעלה של כלי רכב אוטונומיים בכבישים מהירים ונעשה שימוש‬ ‫במודל דינאמי ליניארי אשר מבוסס על הנחה של זוויות היגוי קטנות ונסיעה‬ ‫במהירות ‪ v‬קבועה‪ .‬התכנון בשני המקרים כולל כלים מתחום הבקרה הליניארית‬ ‫כגון שימוש במשערך ובבקר משוב מצב לצורך וויסות הסטייה הצידית והכיוון של‬ ‫כלי הרכב ביחס לנקודת מדידה הנמצאת במרחק נתון לפני כלי הרכב )‪.(look-ahead‬‬ ‫גם ב‪ [Mammar et al. 2001] -‬נעשה שימוש במודל ליניארי המבוסס על הנחה של‬ ‫זוויות היגוי קטנות לצורך בקרת המרחק הצידי של כלי רכב בעל ארבעה גלגלים‬ ‫משולי הכביש‪ .‬במקרה האחרון‪ ,‬כלי הרכב אינו נוסע במהירות קבועה ומתוכנן בקר‬ ‫מסוג ‪ H infinity‬בשיטת תזמון בקרים )‪ ,(speed scheduled‬שיטה זו מאפשרת שימוש‬ ‫בטכניקות המיועדות למערכות ליניאריות קבועות בזמן )למרות העובדה שהמודל‬ ‫תלוי במהירות ) ‪ .( v ( t‬דוגמא נוספת לשימוש במודל ליניארי ניתן למצוא ב‪Zhang ] -‬‬ ‫‪ [et al. 2001‬שם‪ ,‬מלבד העובדה שהמודל המשמש בתכנון מתאים לזוויות היגוי‬ ‫קטנות‪ ,‬נדרשת מהירות קבועה או משתנה לאט ומוצע שם בקר בשיטת ה‪sliding -‬‬ ‫‪ modes‬המבוסס גם על אי וודאות בחלק מהפרמטרים הפיזיקאליים )כגון מקדמי‬ ‫החיכוך של הצמיגים(‪ .‬ב‪ [Tomizuka and Hingwe 1998] -‬נעשה שימוש במודל לא‬ ‫ליניארי המניח זוויות היגוי קטנות ומתוכנן בקר בשיטת ה‪ sliding modes -‬המאפשר‬ ‫עקיבה אחרי קו אמצע )‪ (centreline‬של נתיב )‪ (lane‬בכביש מהיר‪.‬‬ ‫באופן כללי מודל דינאמי של כלי רכב בעל ארבעה גלגלים הינו מודל מורכב אשר‬ ‫מקשה מאוד על תכנון מערכת בקרה‪ .‬מסיבה זו כאשר מעוניינים בנהיגה אוטונומית‬ ‫לאורך מסלול רצוי )לדוגמא עקיבה אחרי שולי הכביש( נהוג לפתח את המודל תוך‬ ‫שימוש בהנחות כגון זוויות היגוי קטנות )ע"י שימוש בקירוב ‪ ( φ " tanφ‬ומהירות‬ ‫נסיעה קבועה )כמו בדוגמאות שהוצגו‪ ,‬למשל‪,[Dickmans & Mysliwetz 1992] :‬‬ ‫]‪ [Kosecka et al. 1998‬ו‪ .([Zhang et al.2001] -‬הנחות אלה מאפשרות פיתוח מודל‬ ‫ליניארי קבוע בזמן ושימוש בטכניקות הבקרה הרבות הקיימות עבור מודלים אלה‪.‬‬ ‫‪12‬‬ ‫כאשר מדובר בכלי רכב הנעים בכבישים מהירים )ללא סיבובים חדים( הנחות אלה‬ ‫עשויות להביא לידי קבלת ביצועים טובים‪ .‬אולם מודלים כאלה עשויים לעוות‬ ‫באופן משמעותי את הקשרים הקינמאטים הקיימים בכלי הרכב כאשר הוא מופעל‬ ‫בזוויות היגוי גדולות‪ .‬כדי להתגבר על בעיה זו‪ ,‬מוצג בעבודה זו מודל קינמטי‪ ,‬לכלי‬ ‫רכב בעל ארבעה גלגלים אשר אינו מבוסס על הנחה של גלגול ללא החלקה )ראה‬ ‫פרק ‪ .(3‬מודל זה אומנם מבוסס על הנחה )מציאותית לגמרי( של זוויות החלקה‬ ‫קטנות אך אינו מצריך כל הנחה לגבי זווית ההגה ‪ . φ‬מבנה המודל כולל חלק אחד‬ ‫אשר זהה למודל השרשרת שנדון בפרק ‪ 2‬של עבודה זו וחלק נוסף אשר ניתן‬ ‫להתייחס אליו כאל הפרעה חסומה‪ .‬הרעיון מאחורי תכנון בקר למודל זה מבוסס על‬ ‫הצורך להקטין ככול האפשר את ההשפעה של ההפרעה )החלקות( על השגיאות של‬ ‫כלי הרכב ביחס למסלול הרצוי‪ .‬הבחירה הטבעית במקרה זה עשויה להיות בקר ∞‪,H‬‬ ‫אבל מכיוון שההפרעה כאן אינה פונקציה ב‪ ,L2 -‬שיטה זו אינה מתאימה‪ .‬שיטה‬ ‫אחרת‪ ,‬המתאימה למערכות ליניאריות קבועות בזמן ונקראת בקרת ‪,peak-to-peak‬‬ ‫ניתן למצוא ב‪ .[Scherer & Weiland 1999] -‬במקרה זה הבסיס לתכנון הבקר הוא‬ ‫מזעור הקריטריון‪:‬‬ ‫∞‬ ‫)‪(1.10‬‬ ‫‪z‬‬ ‫‪dα‬‬ ‫∞‬ ‫‪sup‬‬ ‫∞‪d α ∈L‬‬ ‫‪dα ≠ 0‬‬ ‫כאשר ‪ d α‬היא הפרעה‪ z ,‬הפלט המבוקר של המערכת והנורמה ∞ " מוגדרת באופן‬ ‫הבא‪:‬‬ ‫)‪(1.11‬‬ ‫})‪{ z (t‬‬ ‫‪i‬‬ ‫‪" max sup‬‬ ‫) ∞ ‪i =1,.. p t∈[0 ,‬‬ ‫∞‬ ‫‪z‬‬ ‫הבקר המוצג בפרק ‪ 3‬של עבודת זו מבוסס על הרחבה של בקר ה‪ peak-to-peak -‬מ‪-‬‬ ‫]‪ [Scherer & Weiland 1999‬כך שיתאים למודל הקינמאטי המבוסס על החלקות אשר‬ ‫פותח כאן והינו תלוי בזמן‪ .‬בפרק ‪ 3‬מוצגות גם תוצאות של סימולציות אשר בוצעו‬ ‫באמצעות תוכנה מסוג ‪ CarSim‬המיועדת לביצוע סימולציות דינאמיות של כלי רכב‪.‬‬ ‫תוכנה זו משתמשת במודלים דינאמיים מורכבים ואמינים ומקובלת לשימוש‬ ‫בתעשיית כלי הרכב‪ .‬תוצאות הסימולציה מצביעות על יתרון ברור לבקר שתוכנן‬ ‫בשיטה הנדונה על פני בקר שתוכנן על בסיס מודל קינמטי ללא החלקות‪.‬‬ ‫בהקשר לחלק זה של העבודה‪ ,‬לא ידוע למחבר על כל ניסיון אחר לפתרון בעיית ה‪-‬‬ ‫‪ path following‬בכלי רכב בעל ארבעה גלגלים ע"י שימוש במודלים קינמטיים אשר‬ ‫אינם מבוססים על ההנחה של גלגול ללא החלקה‪ ,‬ופיתוח בקר על פי קריטריון‬ ‫הממזער את ההשפעה של ההחלקות על שגיאות כלי הרכב ביחס למסלול הרצוי‪.‬‬ ‫‪13‬‬ ‫גישה בעלת קווי דמיון מסוימים ניתן למצוא ב‪ [Wang et al. 2004] -‬שם האילוץ הלא‬ ‫הולונומי הנתון ב‪ (1.6) -‬מוחלף באילוץ מהצורה‪:‬‬ ‫) ‪x& sin θ − y& cos θ = δ ( t‬‬ ‫)‪(1.12‬‬ ‫‪,ω >ω‬‬ ‫‪,ω ≤ω‬‬ ‫‪k ω2‬‬ ‫‪δ(t) =  δ‬‬ ‫‪ 0‬‬ ‫כאשר ‪ k δ , ω‬קבועים‪ ,‬ומתוכנן שם בקר לכלי רכב בעל הנע דיפרנציאלית בשיטה‬ ‫‪) neural- networks‬בקר זה שונה במהותו מהבקר המוצע בעבודה זו(‪.‬‬ ‫אחד הקווים המנחים של עבודת המחקר הוא פיתוח אלגוריתמיים אשר יהיו‬ ‫ישימים ומתאימים למימוש בכלי רכב‪ .‬לצורך השגת מטרה זו נבנתה מערכת ניסוי‬ ‫)ראה נספח ‪ (A.1‬ופותחו כלים נוספים אשר ביחד עם הבקרים המוצגים בסעיפים ‪2‬‬ ‫ו‪ 3 -‬מהווים מערכת שלימה וברת מימוש‪ .‬נושא המימוש מוצג בפרק ‪ 4‬וכולל מלבד‬ ‫תיאור של מערכת הניסוי‪ ,‬גם נושאים כגון עיבוד המידע המתקבל מהחיישנים‬ ‫ותכנון מסלול רצוי‪ .‬באופן כללי המודל הקינמטי והדינאמי של כלי רכב בעל ארבעה‬ ‫גלגלים הוא לא ליניארי‪ ,‬לכן הדרך המקובלת לסינון ושיערוך של משתני מצב‬ ‫במקרה זה מבוססת על שימוש במסנן קלמן מורחב )‪ .(EKF‬מספר דוגמאות לכך הן‬ ‫]‪ [Majjar & Bonnifait 2003] ,[Bonnifait et al. 2001‬כאשר שיערוך מצב כלי הרכב‬ ‫מבוסס על מדידות המתקבלות מחיישן מקום מסוג ‪ GPS‬וחישני זווית מסוג‬ ‫אינקודרים המותקנים על הגלגלים )חיישני זווית כאלה הם חלק ממערכת ‪.(ABS‬‬ ‫מסנן קלמן מורחב מבוסס על ליניאריזציה של מודל התהליך ולכן סובל מהעדר‬ ‫הבסיס המתמטי הקיים במקרה של מסנן קלמן )לא מורחב‪ ,‬המתאים למערכות‬ ‫ליניאריות בלבד(‪ .‬כדי להתגבר על חסרון זה מוצגת בפרק ‪ 4‬טרנספורמציה פשוטה‬ ‫המאפשרת פיתוח מסנן ומשערך המבוססים על מודל קינמאטי ליניארי תלוי בזמן‪.‬‬ ‫נושא נוסף הנדון בפרק ‪ 4‬הוא תכנון מסלול רצוי‪ .‬המבנה הקנמאטי המיוחד של כלי‬ ‫רכב בעל אילוצים לא הולונומיים והצורך לאפשר נסיעה רציפה וחלקה מצריך תכנון‬ ‫מסלול בעל תכונות מתאימות )‪ .(feasible‬במקרה של כלי רכב בעל ארבעה גלגלים‪,‬‬ ‫ניתן להראות )מתוך משוואה )‪ ((1.2‬כי זווית הגלגלים הקדמיים ועקמומיות המסלול‬ ‫אשר בו נוסע כלי הרכב מקיימים את הקשר הבא‪:‬‬ ‫)‪(1.13‬‬ ‫‪dθ θ& tan φ‬‬ ‫= =‬ ‫‪ds v‬‬ ‫‪L‬‬ ‫= ) ‪C (s‬‬ ‫כאשר ‪ s‬אורך העקום‪ .‬מכאן ברור כי מסלול רצוי מתאים )‪ (feasible‬לכלי רכב בעל‬ ‫ארבעה גלגלים הוא מסלול בעל רציפות בעקמומיות )מסלול כזה מבטיח רציפות‬ ‫בזווית הגלגלים הקדמיים ‪ .( φ‬דרישה לרציפות באות בקרה מסוג &‪ w = φ‬מצריכה גם‬ ‫‪14‬‬ ‫רציפות בקצב שינוי העקמומיות )לדוגמא ‪ .( dC ( s ) ds‬במקרה שבו המסלול הרצוי‬ ‫נתון ע"י פונקציה מהצורה ) ‪) yd = f ( x‬כמו בחלק מהבקרים המוצגים בפרק ‪(2‬‬ ‫הדרישה האחרונה שקולה לרציפות בנגזרת השלישית של ) ‪ f ( x‬לפי ‪ .x‬במקרה של‬ ‫כלי רכב בעל הנעה דיפרנציאלית דרישה לרציפות באות הבקרה דורשת תכנון‬ ‫מסלול רצוי בעל רציפות בעקמומית בלבד‪.‬‬ ‫מימוש האלגוריתמים המוצגים בפרקים ‪ 2‬ו‪ 3 -‬מצריך מסלול רצוי הניתן לייצוג‬ ‫באופן אנליטי‪ .‬דרישה זו אינה "נוחה" למימוש בתנאי שדה שם מקובל לייצג מסלול‬ ‫רצוי ע"י סידרה של נקודות דרך )‪ (way points‬אשר נקבעות בדרך כלל על סמך‬ ‫מדידות או מודל של השטח )מפה‪ ,‬תצ"א(‪ .‬כדי לאפשר מימוש של הבקרים המוצגים‬ ‫בפרקים ‪ 2‬ו‪ 3 -‬בכלי רכב בעל ארבעה גלגלים )מערכת הניסוי( פותחו שיטות‬ ‫אינטרפולציה המאפשרות תכנון מסלול מתאים )‪ (feasible‬בזמן אמת )‪(real time‬‬ ‫העובר דרך סדרה רצויה של נקודות דרך‪ .‬שיטות אלה מוצגות בפרק ‪) 4‬ובנספחים‬ ‫‪ A.2‬ו‪ (A.3 -‬כאשר בתכנון המסלולים נעשית הפרדה בין בקרים ביחס למערכת‬ ‫קואורדינאטות קבועה במרחב ובקרים ביחס למערכת קואורדינאטות מקומית‪.‬‬ ‫כלים אלה הקשורים בתכנון המסלול הרצוי הינם בעלי אופי יישומי‪ ,‬ופיתוחם הוא‬ ‫תוצאה של הצרכים שהתעוררו במהלך מימוש הבקרים והעדר כלים דומים בעבודות‬ ‫אחרות‪.‬‬ ‫חלק ניקר מתוך התוצאות התיאורטיות המוצגות בעבודה זו נבדק בתנאי סימולציה‬ ‫)חלקים מסוימים נבדקו גם בתנאי ניסוי(‪ .‬כל הסימולציות בוצעו בסביבת ה‪-‬‬ ‫‪ Simulink‬של תוכנת ה‪ .Matlab -‬עקב הכמות הגדולה של דיאגראמות המלבנים‬ ‫)מסביבת ה‪ (Simulink -‬ששימשו לביצוע הסימולציות הן אינן מצורפות לעבודה‬ ‫ומוצגות רק התוצאות הנומריות )באמצעות גרפים(‪.‬‬ ‫‪15‬‬ ‫‪ – 1.3‬תרומת העבודה‬ ‫בסעיף הקודם הוצגה סקירה של עבודות שונות שנעשו בתחום המחקר הנדון‬ ‫ובמקביל לה תוארו הנושאים החדשים שפותחו במסגרת העבודה‪ .‬הפרק הנוכחי‬ ‫מרכז את כל הנושאים החדשים הקיימים בעבודה ומסכם את תרומתם בשטח‬ ‫המחקר הנדון‪.‬‬ ‫מטרת עבודת המחקר הינה פיתוח אלגוריתמים חדשים וברי מימוש המתאימים‬ ‫לפתרון בעיית ה‪ path following -‬בכלי רכב יבשתיים‪ ,‬בעיקר כאלה המבוססים על‬ ‫מבנה מכונית בעלת ארבעה גלגלים‪.‬‬ ‫העבודה מציגה מספר רעיונות וגישות חדשות לפתרון בעיית ה‪.path following -‬‬ ‫בחלק הראשון של העבודה נעשה שימוש במודלים קינמטיים המבוססים על הנחה‬ ‫של גלגול ללא החלקה‪ .‬ידוע כי ניתן להביא מודלים כאלה‪ ,‬ע"י טרנספורמציה‬ ‫מתאימה‪ ,‬לצורה הנקראת צורת שרשרת‪ .‬על פי גישה זו מוצג בעבודה בקר חדש‪ ,‬לא‬ ‫ליניארי‪ ,‬המיועד למערכות אשר ניתנות לייצוג ע"י מודל שרשרת מסדר כלשהו‪.‬‬ ‫יתרונות הבקר שפותח כאן על פני בקרים אחרים המתאימים למודל שרשרת הם‬ ‫היותו בעל אות בקרה חלק והעובדה כי כאשר נעשה בו שימוש לצורך פתרון בעיית‬ ‫ה‪ path following -‬בכלי רכב‪ ,‬הוא יכול למנוע את הצורך באותות בקרה גדולים‬ ‫)בהשוואה לבקרים אחרים( כאשר כלי הרכב נמצא רחוק מהמסלול הרצוי‪.‬‬ ‫שימוש בבקר המתאים למערכות בעלות מודל שרשרת‪ ,‬לצורך פתרון בעיית ה‪path -‬‬ ‫‪ following‬בכלי רכב‪ ,‬מצריך ביצוע טרנספורמציה של מודל כלי הרכב‪.‬‬ ‫הטרנספורמציה אשר מקובל להשתמש בה עבור מקרה זה‪ ,‬סובלת ממצב סינגולארי‬ ‫כאשר כלי הרכב ניצב לציר ‪ x‬של מערכת הצירים )מערכת הצירים שבה נמדד מצב‬ ‫כלי הרכב ונתון המסלול הרצוי(‪ .‬בעבודה זו מוצגת טרנספורמציה שונה אשר אינה‬ ‫סובלת ממצב סינגולארי זה‪ .‬הטרנספורמציה החדשה המוצעת אומנם כוללת מצב‬ ‫סינגולארי כאשר כלי הרכב ניצב למסלול הרצוי‪ ,‬אבל מצב זה אינו תלוי בבחירה‬ ‫השרירותית של מערכת הצירים ונובע מהאופן שבו מוגדרת בעיית העקיבה )כדי‬ ‫לעקוב אחרי המסלול הרצוי‪ ,‬במצב זה‪ ,‬כלי הרכב אינו יודע אם עליו לפנות ימינה‬ ‫או שמאלה(‪ .‬בנוסף לטרנספורמציה זו מוצגות בעבודה גם טרנספורמציות מקוריות‬ ‫נוספות לדוגמא כזאת המבוססת על קואורדינאטות פולאריות וכזאת המבוססת על‬ ‫מערכת קואורדינאטות הצמודה לכלי הרכב )בשני המקרים האחרונים מדובר בכלי‬ ‫רכב בעל ארבעה גלגלים(‪ .‬היתרון הטמון בטרנספורמציות ובגישות החדשות‬ ‫המוצגות‪ ,‬וגם הקו המנחה בפיתוחן‪ ,‬קשור באפשרות לממש את הבקרים‬ ‫המתקבלים בכלי רכב הפועל בתנאי שדה‪ .‬במקרה זה המסלול הרצוי נתון בדרך כלל‬ ‫‪16‬‬ ‫על ידי סידרה של נקודות דרך‪ ,‬ודרושים כלים נוספים המאפשרים תכנון מסלול רצוי‬ ‫ומימוש הבקרים‪ ,‬כלים כאלה פותחו ומוצגים בעבודה‪.‬‬ ‫גישה מעניינת וייחודית לעבודה זו מבוססת על פיתוח מודל בצורת שרשרת‬ ‫המבוסס על גדלים אינווריאנטים בלבד )כלומר כאלה אשר אינם תלויים בבחירה‬ ‫של מערכת צירים(‪ .‬הבקר המתקבל בגישה זו הינו כללי מאוד וניתן לגזור ממנו את‬ ‫כל שאר הבקרים המוצגים בעבודה )עבור מודל כלי רכב ללא החלקות( כמקרים‬ ‫פרטיים‪ .‬היתרון בגישה האחרונה נובע מכך שניתן להפיק בעזרתה בקרים מקוריים‬ ‫)חדשים( נוספים‪ ,‬כלומר כאלה אשר אינם מוצגים בעבודה זו‪.‬‬ ‫פיתוח מערכות בקרה לכלי רכב הנעים במהירות נמוכה מבוסס הרבה פעמים על‬ ‫שימוש במודל קינמאטי המניח גלגול ללא החלקה כפי שמוצג גם בעבודה זו‪.‬‬ ‫כאשר מדובר בכלי רכב אשר נעים בתנאי שדה )לעיתים במהירות גבוהה(‪ ,‬שימוש‬ ‫בבקר אשר תוכנן על בסיס מודל המניח גלגול ללא החלקה עשוי להוביל לביצועים‬ ‫לא מספקים‪ .‬הגישה המקובלת במקרה זה היא שימוש במודל דינאמי אשר מאפשר‬ ‫זוויות החלקה קטנות של הגלגלים‪ ,‬לדוגמא מודל דינאמי מסוג ‪ .bicycle‬עבור כלי‬ ‫רכב בעל ארבעה גלגלים מתקבל מודל דינאמי לא ליניארי מורכב אשר מקשה מאוד‬ ‫על תכנון מערכת בקרה מתאימה‪ .‬כדי להתגבר על מכשול זה נהוג להשתמש‬ ‫בהנחות מסוימות כגון זוויות היגוי קטנות ונהיגה במהירות קווית קבועה‪ ,‬הנחות‬ ‫כאלה מובילות למודל ליניארי‪ .‬בעבודה זו מוצעת גישה חדשה‪ ,‬שונה‪ ,‬אשר אינה‬ ‫מצריכה כל שימוש בהנחות כגון זוויות היגוי קטנות או נהיגה במהירות קבועה‪ .‬על‬ ‫פי הגישה המוצעת מפותח מודל קינמטי אשר אינו מבוסס על הנחה של גלגול ללא‬ ‫החלקה‪ .‬בנוסף מוצע שימוש בטרנספורמציה אשר מובילה למודל אשר כולל חלק‬ ‫אחד בצורת שרשרת וחלק נוסף‪ ,‬התלוי בזוויות ההחלקה של הגלגלים‪ ,‬וניתן‬ ‫להתייחס אליו כאל הפרעה חסומה‪ .‬עבור המודל המתקבל מתוכנן בקר על ידי‬ ‫שימוש בכלים כגון ‪ LMIs‬אשר תפקידו למזער את ההשפעה של ההפרעה )התלויה‬ ‫בהחלקות הגלגלים( על ביצועי מערכת הבקרה )כלומר על השגיאות של מצב כלי‬ ‫הרכב ביחס למסלול הרצוי(‪ .‬על פי סימולציות אשר בוצעו עם תוכנת ‪CarSim‬‬ ‫)המבוססת על מודלים דינאמיים מורכבים ואמינים של כלי רכב( בתנאים בהם‬ ‫קיימות החלקות משמעותיות‪ ,‬מתקבל יתרון ברור לבקר שתוכנן בשיטה המוצעת‬ ‫על פני בקר אחר שתוכנן על בסיס מודל קינמטי ללא החלקות‪.‬‬ ‫נושא נוסף הנדון בעבודה הוא שימוש בחיישן מסוג ‪ GPS‬לצורך מדידת מצב כלי‬ ‫הרכב‪ .‬באופן כללי המודל הקינמטי והדינאמי של כלי רכב בעל ארבעה גלגלים הוא‬ ‫לא ליניארי‪ ,‬לכן הדרך המקובלת לעיבוד המידע המתקבל מחיישן ה‪ GPS -‬מבוססת‬ ‫על שימוש במסנן קלמן מורחב )‪ .(EKF‬מסנן קלמן מורחב מבוסס על ליניאריזציה‬ ‫‪17‬‬ ‫של מודל התהליך ולכן סובל מהעדר הבסיס המתמטי הקיים עבור מסנן קלמן )לא‬ ‫מורחב‪ ,‬המתאים למערכות ליניאריות בלבד(‪ .‬כדי להתגבר על חסרון זה מוצעת‬ ‫בעבודה זו טרנספורמציה חדשה של המודל הקינמטי‪ ,‬המובילה למודל ליניארי תלוי‬ ‫בזמן‪ ,‬ומאפשרת שימוש במסנן קלמן )רגיל‪ ,‬לא מורחב(‪.‬‬ ‫עבודות תיאורטיות רבות בתחום המחקר הנדון מציגות התייחסות אך ורק לפן‬ ‫מסוים של משימת הנהיגה האוטונומית לאורך מסלול רצוי‪ ,‬מבלי להתייחס אל כל‬ ‫המרכיבים הקשורים במימוש ושילוב הבקר במערכת אמיתית‪ .‬יתרונה של עבודה זו‬ ‫על פני עבודות אחרות בתחום קשור בעובדה כי העבודה כאן מציגה פיתוח שלם‪.‬‬ ‫הפיתוח מקיף מגוון נושאים‪ ,‬החל מהשלב בו מתוכנן בקר המתאים לנהיגה‬ ‫אוטונומית על מסלול המוגדר על ידי פונקציה )באופן אנליטי(‪ ,‬ועד לשלב בו ניתן‬ ‫לממש את מערכת הבקרה בכלי רכב חשמלי הכולל חיישנים שונים ונדרש לנהוג‬ ‫באופן אוטונומי לאורך מסלול רצוי המוגדר על ידי סדרה של נקודות דרך )כמקובל‬ ‫בבעיה מעשית(‪ .‬כדי להוכיח את ישימות ויעילות השיטה נבנה )במסגרת עבודת‬ ‫המחקר( כלי רכב חשמלי המבוסס על מבנה קינמאטי דומה לזה של מכונית בעלת‬ ‫ארבעה גלגלים ובוצעו ניסויים אשר חלקם מתועדים כאן בהמשך‪.‬‬ ‫‪18‬‬ ‫‪ -2‬תכנון בקרים לכלי רכב אוטונומיים המבוססים על שימוש במודל שרשרת‬ ‫בפרק ‪) 1‬הקדמה( הוצגו המודלים הקינמטיים של כלי רכב בעל הנעה דיפרנציאלית וכלי‬ ‫רכב בעל ארבעה גלגלים‪ .‬האפשרות לייצג מודלים אלה בצורות שרשרת מסדר ‪ 2‬ו‪3 -‬‬ ‫מהווה תמריץ לפיתוח בקר כללי למערכות אשר ניתן לייצג את המודל המתמטי שלהן‬ ‫בצורת שרשרת מסדר ‪ , n‬פיתוח בקר כזה מוצג בסעיף ‪ 2.1‬ובסעיפים ‪ 2.2 - 2.5‬מוצגים‬ ‫מימושים שונים של הבקר הכללי בכלי רכב אוטונומיים‪.‬‬ ‫‪ – 2.1‬פיתוח בקר למערכות הניתנות לייצוג בצורת שרשרת‬ ‫מודלים של מערכות לא הולונומיות רבות )כמו כלי רכב( ניתן להביא ע"י טרנספורמציה‬ ‫לצורה הנקראת צורת שרשרת‪ .‬עקב המבנה המיוחד של ייצוג זה הוא מהווה מסגרת‬ ‫נוחה לתכנון מערכות בקרה‪ .‬ע"פ גישה זו מוצג בסעיף זה בקר למערכת ‪ n‬ממדית בעלת‬ ‫כניסה אחת הניתנת לייצוג בצורת שרשרת כפי שנתון במשוואה )‪.(2.1.1‬‬ ‫) ‪e&1 = e2 v ( t‬‬ ‫) ‪e& 2 = e3 v ( t‬‬ ‫)‪(2.1.1‬‬ ‫‪M‬‬ ‫‪M‬‬ ‫) ‪e& n −1 = e n v ( t‬‬ ‫‪e& n = u‬‬ ‫כאשר ∞ < ‪) . v ( t ) ∈ R , 0 < η1 ≤ v ( t ) ≤ η2‬בהמשך ) ‪ v ( t‬הינו אות נתון הקשור‬ ‫במהירות כלי הרכב ומשתני המצב ) ‪ ei ( t‬מתארים שגיאות הקשורות במצב של כלי‬ ‫הרכב ביחס למסלול רצוי(‪ .‬תפקיד הבקר ) ‪ u ( t‬הוא להביא את משתני המצב לאפס‪.‬‬ ‫למערכת )‪ (2.1.1‬מוצע הבקר הבא‪:‬‬ ‫)‪(2.1.2‬‬ ‫‪u ( t ) = − ( kh ( σ ) + a1e 2 + a 2e3 + K + a n −1en ) v ( t ) , k > 0‬‬ ‫כאשר‪:‬‬ ‫)‪(2.1.3‬‬ ‫) ‪σ ( t ) = a1e1 ( t ) + a 2e2 ( t ) + K + a n −1e n −1 ( t ) + e n ( t‬‬ ‫ו‪ h : R → R -‬היא פונקציה רציפה וממשית המקיימת‪:‬‬ ‫‪19‬‬ ‫‪σ h ( σ ) > 0 , ∀σ ∈ R , σ ≠ 0‬‬ ‫לצורך הוכחת היציבות של הבקר )ראה משפט ‪ (2.2‬דרוש המשפט הבא‪:‬‬ ‫משפט ‪ :2.1‬נתונה מערכת ליניארית תלויה בזמן‪:‬‬ ‫∞ < ‪e& ( t ) = ( Ae ( t ) + Bσ ( t ) ) v ( t ) , e ( t ) ∈ R n −1 , v ( t ) : [ 0, ∞ ) → R , 0 < η1 ≤ v ( t ) ≤ η2‬‬ ‫כאשר ) ‪ v ( t‬הינה אינטגרבילית על ] ‪ [ 0, t‬לכל ‪. t‬‬ ‫אם כל הערכים העצמיים של ‪ A‬נמצאים בחצי השמאלי של המישור הקומפלקסי ו‪-‬‬ ‫‪ lim σ ( t ) = 0‬באופן אקספוננציאלי )כלומר קיימים קבועים חיוביים ‪ σ0‬ו‪ ρ -‬כך‬ ‫∞→ ‪t‬‬ ‫שמתקיים ) ‪ .( σ ( t ) ≤ σ0exp ( -ρ t‬אזי ‪ lim e (x ) = 0‬באופן אקספוננציאלי‪.‬‬ ‫∞→ ‪t‬‬ ‫הוכחה‪:‬‬ ‫נראה תחילה כי קיימים קבועים ‪ c1 ≥ 1‬ו‪ c2 > 0 -‬כך ש‪:‬‬ ‫)‪(2.1.4‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫‪exp  A ∫ v ( ζ ) dζ  ≤ c1 exp ( −c2 ( t − τ ) ) , ∀t ≥ τ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ τ‬‬ ‫‪‬‬ ‫תהיה נתונה המערכת ההומוגנית‪:‬‬ ‫) ‪e& ( t ) = Ae ( t ) v ( t‬‬ ‫)‪(2.1.5‬‬ ‫קל לראות שבמקרה זה מתקיים‪:‬‬ ‫)‪(2.1.6‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫‪A ⋅ exp  A ∫ v ( ζ ) dζ  = exp  A ∫ v ( ζ ) dζ  ⋅ A‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ τ‬‬ ‫‪‬‬ ‫‪ τ‬‬ ‫‪‬‬ ‫לכן הפתרון של המערכת ההומוגנית )‪ (2.1.5‬נתון ע"י‪:‬‬ ‫)‪(2.1.7‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫) ‪e ( t ) = exp  A ∫ v ( ζ ) dζ  e ( τ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ τ‬‬ ‫‪‬‬ ‫נגדיר פונקצית ליאפונוב‪:‬‬ ‫‪V ( e ) = e T Pe , P = P T > 0‬‬ ‫הנגזרת של ‪ V‬לאורך ) ‪: e ( t‬‬ ‫)‬ ‫(‬ ‫‪& = e T A T P + PA ( v ( t ) ) e‬‬ ‫‪V‬‬ ‫מכיוון ש‪ A -‬היא מטריצה יציבה‪ ,‬אז עבור כל ‪ , Q = QT > 0‬קיים פתרון יחיד‬ ‫‪ P = PT > 0‬המקיים את משוואת ליאפונוב הבאה‪:‬‬ ‫‪20‬‬ ‫‪A T P + PA = −Q‬‬ ‫מכאן ברור כי‪:‬‬ ‫‪2‬‬ ‫)‬ ‫(‬ ‫‪& = − e T Qe ( v ( t ) ) ≤ −λ ( Q ) η e‬‬ ‫‪V‬‬ ‫‪1‬‬ ‫‪min‬‬ ‫בנוסף‪ ,‬מתוך ההגדרה של ‪ V‬ברור כי‪:‬‬ ‫‪2‬‬ ‫‪≤ V ≤ λ max ( P ) e‬‬ ‫‪2‬‬ ‫‪λmin ( P ) e‬‬ ‫לכן הפתרון )‪ (2.1.7‬של המערכת ההומוגנית דועך לאפס באופן אקספוננציאלי‪ .‬מכיוון‬ ‫שטענה זו נכונה לכל תנאי התחלה ) ‪ e ( τ‬אז ברור כי קיימים קבועים ‪ c1 ≥ 1‬ו‪c2 > 0 -‬‬ ‫המקיימים את )‪.(2.1.4‬‬ ‫נחזור למערכת הנתונה במשפט‪:‬‬ ‫מתוך )‪ (2.1.5‬ו‪ (2.1.7) -‬ברור כי הפתרון של המערכת הנתונה במשפט הוא‪:‬‬ ‫⇒‬ ‫)‪(2.1.8‬‬ ‫‪t‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫‪e ( t ) = exp  A ∫ v ( ζ ) dζ  e ( 0 ) + ∫ exp  A ∫ v ( ζ ) dζ Bσ ( τ ) v ( τ ) dτ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪ 0‬‬ ‫‪‬‬ ‫‪ τ‬‬ ‫‪‬‬ ‫‪t‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫‪e ( t ) ≤ exp  A ∫ v ( ζ ) dζ  e ( 0 ) + ∫ exp  A ∫ v ( ζ ) dζ  B σ ( τ ) v ( τ ) dτ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪ 0‬‬ ‫‪‬‬ ‫‪ τ‬‬ ‫‪‬‬ ‫נשתמש בתוצאה )‪:(2.1.4‬‬ ‫‪ t‬‬ ‫‪‬‬ ‫) ) ‪exp  A ∫ v ( ζ ) dζ  ≤ c1 exp ( −c2 ( t − τ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ τ‬‬ ‫‪‬‬ ‫)‪(2.1.9‬‬ ‫כאשר ‪ c2 > 0‬ו‪) , c1 > 1 -‬כאן יש לבחור ‪.( c2 ≠ ρ‬‬ ‫מתוך )‪ (2.1.9) ,(2.1.8‬והנתונים ) ‪ 0 < η1 ≤ v ( t ) ≤ η2 < ∞ , σ ( t ) ≤ σ0exp ( -ρ t‬ברור כי‪:‬‬ ‫‪t‬‬ ‫‪e ( t ) ≤ c1 exp ( −c2 t ) e ( 0 ) + c1 B σ0η2 ∫ exp ( −c 2 ( t − τ ) ) exp ( −ρτ ) dτ‬‬ ‫‪0‬‬ ‫)‪(2.1.10‬‬ ‫‪( c2 − ρ ) ≠ 0‬‬ ‫‪c B σ0η2‬‬ ‫‪= c1 exp ( −c2 t ) e ( 0 ) + 1‬‬ ‫‪( exp ( −ρt ) − exp ( −c2 t ) ) ,‬‬ ‫) ‪( c2 − ρ‬‬ ‫נסמן‪:‬‬ ‫} ‪ρ = min {ρ, c 2‬‬ ‫מתוך )‪(2.1.10‬‬ ‫‪21‬‬ ‫‪‬‬ ‫‪B σ0η2 ‬‬ ‫‪e ( t ) ≤  e (0) +‬‬ ‫) ‪ c1 exp ( −ρ t‬‬ ‫‪‬‬ ‫‪c‬‬ ‫‬‫‪ρ‬‬ ‫(‬ ‫)‬ ‫‪2‬‬ ‫‪‬‬ ‫‪‬‬ ‫הערות‪:‬‬ ‫‪ .1‬ניתן למצוא משפטים העוסקים בחיבור טורי של מערכות לא ליניאריות מהצורה‪:‬‬ ‫‪σ‬‬ ‫) ‪e& = f 2 ( e , σ‬‬ ‫) ‪σ& = f1 ( σ‬‬ ‫כאשר ‪) f1 ( 0 ) = f 2 ( 0, 0 ) = 0‬ראה לדוגמה ]‪.([Isidori 1995‬‬ ‫במקרה זה יציבות אסימפטוטית מקומית של נקודת שיווי המשקל ‪ e = 0‬במערכת‬ ‫) ‪) e& = f 2 ( e , 0‬המערכת ‪ f 2‬מוזנת ע"י ‪ ( σ = 0‬ויציבות אסימפטוטית מקומית של נקודת‬ ‫שיווי המשקל ‪ σ = 0‬במערכת ) ‪ σ& = f1 ( σ‬מבטיחים יציבות אסימפטוטית מקומית של‬ ‫נקודת שיווי המשקל ) ‪ ( e , σ ) = ( 0, 0‬במערכת הטורית )המורכבת מ‪ f1 -‬ו‪ .( f 2 -‬קיימים‬ ‫גם תנאים נוספים המבטיחים יציבות אסימפטוטית גלובלית‪ .‬העובדה שכאן בחרנו‬ ‫להציג הוכחה מפורטת עבור המקרה הליניארי ולא להתבסס על מקרה פרטי של‬ ‫תיאוריה זו עבור מערכות לא לניאריות‪ ,‬נועדה כדי להוכיח התכנסות אקספוננציאלית‬ ‫)המהווה תכונה חזקה יותר מהתכנסות אסימפטוטית( אשר תשמש בסיס לפיתוח‬ ‫בקרים בהמשך‪.‬‬ ‫‪ .2‬ב‪ [Sørdalen & Egeland 1995] -‬מוצג בקר למערכות אשר ניתנות לייצוג ע"י שימוש‬ ‫במודל שרשרת ואשר מתאים לפתרון בעיית ה‪ set point -‬בכלי רכב אוטונומיים‪ .‬פיתוח‬ ‫הבקר שם מתבסס על משפט הכולל תנאים להתכנסות אקספוננציאלית של מערכת חד‬ ‫ממדית מהצורה ) ‪ . e& = − f1 ( e , t ) e + f 2 ( e , t‬חלק מהתנאים במשפט הם‪ :‬דעיכה‬ ‫אקספוננציאלית של הכניסה ) ‪ f 2 ( e , t‬וקיום התכונה‬ ‫‪∫t ( f1 ( e ( τ ) , τ ) − λ ) dτ ≤ p, ∀t ≥ t 0‬‬ ‫‪t‬‬ ‫‪0‬‬ ‫כאשר ‪ p‬ו‪ λ -‬הם קבועים חיוביים‪.‬‬ ‫למרות דמיון מסוים בין המשפט ב‪ [Sørdalen & Egeland 1995] -‬ומשפט ‪ ,2.1‬העובדה כי‬ ‫כאן נדרש משפט למערכת ‪ n‬ממדית חייבה את הכתיבה וההוכחה של משפט ‪.2.1‬‬ ‫‪22‬‬ ‫משפט ‪ :2.2‬הנח ‪ Re(λ i ) < 0, i = 1, K n‬כאשר ‪ λ i‬הם השורשים של הפולינום‬ ‫‪ . λn −1 + a n −1λn −2 + K + a 1‬אזי שימוש בבקר הנתון ב‪ (2.1.2) -‬מבטיח ‪lim e ( t ) = 0‬‬ ‫∞→ ‪t‬‬ ‫הוכחה‪:‬‬ ‫מתוך הצבה של ‪ u‬ב‪ σ& -‬מתקבל‪:‬‬ ‫‪σ& ( t ) = a1e&1 + a 2e& 2 + K + a n −1e& n −1 + e& n‬‬ ‫‪σ& ( t ) = a1e2 v ( t ) + a 2e3 v ( t ) + K + a n −1e n v ( t ) + u‬‬ ‫)‪(2.1.11‬‬ ‫) ‪σ& ( t ) = − kh ( σ ) v ( t‬‬ ‫נגדיר פונקצית ליאפונוב ) ‪ V(t‬באופן הבא‪:‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫) ‪σ(t‬‬ ‫‪2‬‬ ‫= )‪V(t‬‬ ‫מגזירה של ) ‪ V(t‬והצבה של )‪ (2.1.11‬מתקבל‪:‬‬ ‫‪& ( t ) = σσ& = − kσ h ( σ ) v ( t ) < 0, ∀σ ≠ 0‬‬ ‫‪V‬‬ ‫מכאן ברור כי ‪ lim σ ( t ) = 0‬באופן אקספוננציאלי ו‪. max σ ( t ) = σ ( 0 ) -‬‬ ‫‪t‬‬ ‫∞→ ‪t‬‬ ‫מתוך )‪(2.1.3‬‬ ‫)‪(2.1.12‬‬ ‫) ‪en ( t ) = −a1e1 ( t ) − a 2e 2 ( t ) − K − a n −1e n −1 ( t ) + σ ( t‬‬ ‫מתוך הצבה של )‪ (2.1.12‬ב‪ ( n − 1) -‬המשוואות הראשונות של המודל )‪:(2.1.1‬‬ ‫) ‪e&1 = e 2 v ( t‬‬ ‫) ‪e&1 = e 2 v ( t‬‬ ‫)‪(2.1.13‬‬ ‫‪M‬‬ ‫‪M‬‬ ‫) ‪e& n −1 = ( −a1e1 − a 2e 2 − K − a n −1e n −1 + σ ( t ) ) v ( t‬‬ ‫על ידי שימוש בהגדרה ‪ e = [e1 L e n −1 ]T‬המערכת )‪ (2.1.13‬ניתנת לרישום גם באופן‬ ‫הבא‪:‬‬ ‫‪0 ‬‬ ‫‪0‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪0 ‬‬ ‫‪ ‬‬ ‫‪O‬‬ ‫) ‪0  e v ( t ) +  M  σ ( t ) v ( t ) = ( Ae + Bσ ( t ) ) v ( t‬‬ ‫‪‬‬ ‫‪ ‬‬ ‫‪1 ‬‬ ‫‪0‬‬ ‫‪‬‬ ‫‪1 ‬‬ ‫‪L −a n −1 ‬‬ ‫‪L‬‬ ‫)‪(2.1.14‬‬ ‫‪23‬‬ ‫‪0‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪−a 3‬‬ ‫‪1‬‬ ‫‪ 0‬‬ ‫‪ 0‬‬ ‫‪0‬‬ ‫‪‬‬ ‫‪M‬‬ ‫‪e& =  M‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪ 0‬‬ ‫‪ −a1 −a 2‬‬ ‫המערכת )‪ (2.1.14‬היא מערכת ליניארית תלויה בזמן ומתוך התנאי במשפט ‪2.2‬‬ ‫המטריצה ‪ A‬יציבה‪ .‬מכיוון ש‪ lim σ ( t ) = 0 -‬באופן אקספוננציאלי‪ ,‬מתוך משפט ‪2.1‬‬ ‫∞→ ‪t‬‬ ‫‪ lim e ( t ) = 0‬באופן אקספוננציאלי ומתוך ההגדרה של ) ‪ σ ( t‬מתקבל‪. lim e(t ) = 0 :‬‬ ‫∞→ ‪t‬‬ ‫∞→ ‪t‬‬ ‫הערות לגבי סעיף ‪:2.1‬‬ ‫‪ .1‬כאשר ‪) −∞ < η1 ≤ v ( t ) ≤ η2 < 0‬בהמשך‪ ,‬נתון זה מהווה ייצוג של מהירות שלילית(‬ ‫ניתן להוכיח יציבות )בדרך דומה ל‪" -‬מקרה החיובי"( אם מתקיים ‪ k < 0‬ו‪-‬‬ ‫‪ Re(λ i ) > 0, i = 1,K n‬כאשר ‪ λ i‬הם השורשים של הפולינום ‪. λn −1 + a n −1λn −2 + K + a 1‬‬ ‫‪ .2‬כאשר ‪ v ( t ) = 0‬אז ‪ u = 0‬ו‪ . e(t ) = e = const -‬מצב זה מתייחס למקרה בו מהירות‬ ‫כלי הרכב שווה אפס‪ ,‬במצב זה התכנסות למסלול הרצוי אינה אפשרית‪ .‬איפוס אות‬ ‫הבקרה יבטיח בהמשך מצב סטטי של מערכת ההגה כאשר מהירות כלי הרכב שווה‬ ‫אפס‪.‬‬ ‫‪ .3‬מקרה פרטי של הבקר המוצע הינו כאשר הפונקציה ) ‪ h ( σ‬הינה קומבינציה ליניארית‬ ‫של משתני המצב‪ .‬במקרה זה מדובר בבקר משוב מצב מהצורה )בקר כזה נתון ב‪-‬‬ ‫]‪:([Samson 1995‬‬ ‫)‪(2.1.15‬‬ ‫) ‪u ( t ) = − ( k1e1 + k 2e2 + K + k n en ) v ( t‬‬ ‫קל להראות ע"י שימוש בפונקצית ליאפונוב מהצורה ‪ V ( t ) = eT Pe , P = P T > 0‬כי‬ ‫במקרה זה בחירה של קבועי הבקר ‪ k i‬כך שכל השורשים של הפולינום‬ ‫‪ λ n + k n λ n −1 + k n −1λ n − 2 + K + k1‬נמצאים בצד השמאלי של המישור הקומפלקסי‬ ‫מבטיחה יציבות אסימפטוטית של החוג הסגור‪ .‬היתרון של הבקר הכללי המוצע כאן‬ ‫הוא בכך שניתן לבחור פונקציה ) ‪ h ( σ‬חסומה )לדוגמה ) ‪ .( h ( σ ) = tanh ( σ‬מכיוון‬ ‫שמשתנה המצב ‪ e1‬מופיע באות הבקרה אך ורק בתוך הפונקציה ) ‪ , h ( σ‬ניתן ע"י בחירה‬ ‫של ) ‪ h ( σ‬חסומה למנוע שימוש באותות בקרה גבוהים )אשר לא ניתנים למימוש(‬ ‫כאשר ערכו המוחלט של משתנה המצב ‪ e1‬הינו גבוה‪ ,‬וזאת מבלי להקטין את הגברי‬ ‫הבקר‪ .‬בהמשך העבודה משתנה המצב ‪ e1‬מהווה ייצוג של שגיאת המקום של כלי הרכב‬ ‫ביחס למסלול הרצוי‪.‬‬ ‫‪24‬‬ ‫ מימוש הבקר המוצע בכלי רכב אוטונומיים מאפשר הוכחת יציבות מקומית בלבד‬.4 :‫ומצריך שימוש בחסמים הבאים‬ :(2.2 ‫ )הוכחה‬max σ ( t ) = σ ( 0 ) ‫( והעובדה כי‬2.1.3) -‫ ב‬σ ( t ) ‫מתוך ההגדרה של‬ t en ( t ) = σ ( t ) − a1e1 ( t ) + a 2e 2 ( t ) + K + a n −1en −1 ( t )  ⇒ en ( t ) ≤ σ ( t ) + a1e1 ( t ) + a 2e 2 ( t ) + K + a n −1en −1 ( t ) (2.1.16) ≤ σ ( 0 ) + a1e1 ( t ) + a 2e2 ( t ) + K + a n −1en −1 ( t ) (2.1.14) -‫ המוגדרים ב‬B -‫ ו‬A ‫עבור‬ 1  0  0 0  −1 A B= M M  0  0  −a1 −a 2 0 1 0 −a 3 −1 0  0   −1 a1  0  0   0      0  M  =  M  O      1  0   0  L −a n −1  1   0  L ‫ נבחרים כך‬a1 , a 2 ,L , a n −1 ‫התוצאה האחרונה מבוססת על העובדה שהמקדמים‬ .‫ אינה סינגולרית‬A ‫ והמטריצה‬a1 > 0 ‫ לכן‬,‫ בעלת ערכים עצמיים יציבים‬A ‫שהמטריצה‬ -‫ המוגדרים ב‬B -‫ ו‬A ‫ ועבור‬max σ ( t ) = σ ( 0 ) ‫ העובדה כי‬,(2.1.8) ‫מתוך הפתרון‬ t :(2.1.14)  t  e ( t ) ≤ exp  A ∫ v ( ζ ) dζ  e ( 0 ) + σ ( 0 )    0  t   t   ∫ exp  A ∫ v ( ζ ) dζ v ( τ ) dτ  B      τ  0   t  = exp  A ∫ v ( ζ ) dζ  e ( 0 ) + σ ( 0 )    0    t    exp  A ∫ v ( ζ ) dζ  − I  A −1B      0     t  ≤ exp  A ∫ v ( ζ ) dζ  e ( 0 ) + σ ( 0 )    0    t    exp  A ∫ v ( ζ ) dζ  − I  A −1B      0     t  σ (0) = exp  A ∫ v ( ζ ) dζ  e ( 0 ) +   a1  0    t    exp  A ∫ v ( ζ ) dζ  − I       0     t  σ ( 0)   ≤ exp  A ∫ v ( ζ ) dζ  e ( 0 ) +   a  1  0   25    t   exp  A ∫ v ( ζ ) dζ   + 1      0    (2.1.17) ‫בגלל התכונות הקשורות בפונקציה ) ‪) v ( t‬לדוגמא‪ ,( 0 < η1 ≤ v ( t ) ≤ η2 < ∞ :‬ברור כי‬ ‫‪ t‬‬ ‫‪‬‬ ‫) ‪sup exp  A ∫ v ( ζ ) dζ  = sup exp ( At‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪t‬‬ ‫‪t‬‬ ‫‪ 0‬‬ ‫‪‬‬ ‫)‪(2.1.18‬‬ ‫נגדיר את החסם הבא‪:‬‬ ‫‪sup exp ( At ) = c3‬‬ ‫)‪(2.1.19‬‬ ‫‪t‬‬ ‫כאשר ברור כי עבור מטריצה ‪ A‬יציבה‪ ,‬הקבוע ‪ c3‬הינו סופי וניתן לחישוב‪.‬‬ ‫מתוך הצבה של )‪ (2.1.18-19‬ב‪ (2.1.17) -‬מתקבל‪:‬‬ ‫)‪( c3 + 1‬‬ ‫)‪(2.1.20‬‬ ‫על ידי שימוש בהגדרות‬ ‫)‪σ ( 0‬‬ ‫‪a1‬‬ ‫‪e ( t ) ≤ c3 e ( 0 ) +‬‬ ‫] ‪ , e ( t ) = e1 ( t ) L e n −1 ( t ) , a = [ a1 L a n −1‬בחסם‬ ‫‪T‬‬ ‫‪T‬‬ ‫)‪ (2.1.20‬וב‪:Schwarz inequality -‬‬ ‫‪‬‬ ‫‪‬‬ ‫)‪σ(0‬‬ ‫‪a T e ( t ) ≤ a  c3 e ( 0 ) +‬‬ ‫‪c3 + 1) ‬‬ ‫(‬ ‫‪‬‬ ‫‪‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫‪‬‬ ‫)‪(2.1.21‬‬ ‫מתוך )‪ (2.1.16‬ו‪:(2.1.21) -‬‬ ‫)‪(2.1.22‬‬ ‫‪‬‬ ‫‪‬‬ ‫)‪σ ( 0‬‬ ‫‪e n ( t ) ≤ σ ( 0 ) + a  c3 e ( 0 ) +‬‬ ‫‪c3 + 1) ‬‬ ‫(‬ ‫‪‬‬ ‫‪‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪( c + 1)  σ 0 + c a e 0‬‬ ‫‪en ( t ) ≤ 1 + a 3‬‬ ‫) (‬ ‫) ( ‪‬‬ ‫‪3‬‬ ‫‪a1 ‬‬ ‫‪‬‬ ‫בהמשך פרק זה מוצגים מימושים של בקר )‪ ,(2.1.2‬המיועדים לנהיגה אוטונומית לאורך‬ ‫מסלול רצוי של כלי רכב בעל הנעה דיפרנציאלית וכלי רכב בעל ארבעה גלגלים‪.‬‬ ‫מימושים אלה מבוססים על מודלים בצורת שרשרת מסדר שני ושלישי ולכן מצריכים‬ ‫שימוש מתאים בחסמים )‪ (2.1.20‬ו‪ .(2.1.22) -‬עבור מערכת בצורת שרשרת מסדר שני‪:‬‬ ‫‪, a = a1 , a1 > 0 , c3 = 1‬‬ ‫) ‪e ( 0 ) = e1 ( 0‬‬ ‫‪,‬‬ ‫) ‪σ ( 0 ) = a1e1 ( 0 ) + e2 ( 0‬‬ ‫⇒‬ ‫) ‪a1e1 ( 0 ) + e2 ( 0‬‬ ‫)‪(2.1.23‬‬ ‫‪a1‬‬ ‫‪e1 ( t ) ≤ e1 ( 0 ) + 2‬‬ ‫) ‪e2 ( t ) ≤ 3 a1e1 ( 0 ) + e 2 ( 0 ) + a1 e1 ( 0‬‬ ‫‪26‬‬ ‫עבור מערכת בצורת שרשרת מסדר שלישי‪ ,‬מתקבל מתוך )‪ (2.1.20‬ו‪.(2.1.22) -‬‬ ‫‪, a = [ a1 a 2 ] , a1 , a 2 > 0‬‬ ‫) ‪e1 ( 0‬‬ ‫) ‪e2 ( 0‬‬ ‫= )‪e (0‬‬ ‫‪,‬‬ ‫) ‪σ ( 0 ) = a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0‬‬ ‫⇒‬ ‫) ‪a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0‬‬ ‫‪a1‬‬ ‫)‪(2.1.24‬‬ ‫)‪+ ( c3 + 1‬‬ ‫) ‪e1 ( 0‬‬ ‫) ‪e2 ( 0‬‬ ‫) ‪a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0‬‬ ‫) ‪e1 ( 0‬‬ ‫)‪+ ( c3 + 1‬‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪e1 ( t ) ≤ c3‬‬ ‫‪e 2 ( t ) ≤ c3‬‬ ‫‪ ( c + 1) a 2 + a 2‬‬ ‫‪‬‬ ‫)‪e (0‬‬ ‫‪3‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪‬‬ ‫≤ ) ‪e3 ( t‬‬ ‫‪+ 1 a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1‬‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ – 2.2‬תכנון בקרים לכלי רכב אוטונומיים בגישה של מערכת קואורדינאטות קרטזית‬ ‫קבועה במרחב‬ ‫בסעיף זה יוצגו מספר בקרים המיועדים למימוש בכלי רכב בעלי ארבעה גלגלים כאשר‬ ‫משותפת לכולם העובדה שמודל השגיאות המתאר את מצב כלי הרכב ביחס למסלול‬ ‫הרצוי מוגדר ביחס למערכת קואורדינאטות קרטזית קבועה במרחב )מערכת‬ ‫אינרציאלית(‪ .‬במקרה הנדון בפרק זה מעוניינים בנהיגה אוטונומית לאורך מסלול רצוי‬ ‫הנתון ע"י פונקציה מהצורה ) ‪ yd = f ( x‬רציפה וגזירה לפחות שלוש פעמים )כפי שנתון‬ ‫באיור ‪ .(2.2.1‬במהלך הפרק תוצג גם דוגמה מייצגת של פיתוח בקר המתאים לכלי רכב‬ ‫בעל הנעה דיפרנציאלית על פי אחת מהגישות המוצעות כאן לכלי רכב בעל ארבעה‬ ‫גלגלים‪.‬‬ ‫‪φ‬‬ ‫‪v‬‬ ‫‪θ‬‬ ‫‪y‬‬ ‫)‪f (x‬‬ ‫‪yd‬‬ ‫‪x‬‬ ‫איור ‪ – 2.2.1‬מודל כלי רכב בעל ארבעה גלגלים במערכת קואורדינאטות קרטזית‬ ‫‪27‬‬ ‫נרשום שוב את המודל הקינמאטי של כלי רכב בעל ארבעה גלגלים הנתון ב‪(1.2) -‬‬ ‫‪x& = cos θ v‬‬ ‫‪y& = sin θ v‬‬ ‫‪tan φ‬‬ ‫‪v‬‬ ‫= &‪θ‬‬ ‫‪L‬‬ ‫‪φ& = w‬‬ ‫לכל הבקרים המוצגים בפרק זה משותפת העובדה כי טרנספורמציה מתאימה מובילה‬ ‫למודל שגיאות אשר מאפשר‪ ,‬ע"י שימוש באות בקרה מתאים‪ ,‬קבלת מודל ליניארי תלוי‬ ‫בזמן בצורת שרשרת‪ .‬תהליך זה נקרא ‪ feedback linearization‬והמשך הפיתוח מבוסס על‬ ‫שימוש בתיאוריה המוצגת בסעיף ‪ .2.1‬שימוש בטכניקה המבוססת על טרנספורמציה‬ ‫ואות בקרה מתאים המובילים למודל שרשרת‪ ,‬לצורך פיתוח בקרים לכלי רכב‬ ‫אוטונומיים אינו חדש בספרות המדעית‪ .‬ככל שידוע למחבר עבודה זו‪ ,‬הטרנספורמציה‬ ‫היחידה המקובלת לשימוש‪ ,‬כאשר המטרה היא פתרון בעיית ה‪ path following -‬עבור‬ ‫כלי רכב בעל ארבעה גלגלים בגישה של מערכת קואורדינאטות קרטזית קבועה במרחב‪,‬‬ ‫נתונה באופן הבא )ראה לדוגמא‪ [Samson 1995] :‬ו‪:([Lu et al. 2000] -‬‬ ‫‪e1 = f (x ) − y‬‬ ‫‪e 2 = f ′(x ) − tan θ‬‬ ‫)‪(2.2.1‬‬ ‫‪tan φ‬‬ ‫‪L cos 3 θ‬‬ ‫‪e 3 = f ′′(x ) −‬‬ ‫כאשר ‪ e1‬שגיאת מקום‪ e 2 ,‬שגיאת כיוון ו‪ e 3 -‬שגיאה בכיוון הגלגלים הקדמיים של כלי‬ ‫הרכב‪ .‬טרנספורמציה זו מוגדרת רק כאשר ‪ . θ , φ < π 2 − ε , ε > 0‬מגבלת הסיבוב של‬ ‫הגלגלים הקדמיים ‪ , φ‬קשורה במבנה המכאני של כלי הרכב ולא ניתן להימנע ממנה‪.‬‬ ‫המגבלה הקשורה בכיוון כלי הרכב ‪ , θ‬הינה תולדה של שימוש בטרנספורמציה )‪.(2.2.1‬‬ ‫בהמשך סעיף זה נדונים החסרונות של הטרנספורמציה )‪ (2.2.1‬ומוצגות טרנספורמציות‬ ‫אחרות‪ ,‬בעלות יתרונות ברורים‪ ,‬אשר פותחו במסגרת עבודת מחקר זו‪.‬‬ ‫מגזירת )‪ (2.2.1‬והצבה של מודל כלי הרכב )‪ (1.2‬תתקבל המערכת הבאה‪:‬‬ ‫)‪(2.2.2‬‬ ‫&‪e& 1 = e 2 x‬‬ ‫&‪e& 2 = e 3 x‬‬ ‫‪‬‬ ‫‪3 tan 2 φ tan θ ‬‬ ‫‪w‬‬ ‫‪ x& −‬‬ ‫‪e& 3 =  f ′′′(x ) −‬‬ ‫‪2‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪‬‬ ‫‪L cos θ ‬‬ ‫‪L cos φ cos 3 θ‬‬ ‫‪‬‬ ‫שימוש באות בקרה מהצורה‪:‬‬ ‫‪28‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪3 tan 2 φ tan θ ‬‬ ‫‪‬‬ ‫‪ x& − u  L cos 2 φ cos 3 θ‬‬ ‫‪′‬‬ ‫‪′‬‬ ‫‪′‬‬ ‫=‬ ‫‪−‬‬ ‫) ‪w  f (x‬‬ ‫‪L2 cos 4 θ ‬‬ ‫‪‬‬ ‫‪‬‬ ‫)‪(2.2.3‬‬ ‫מוביל למודל שגיאות בצורת שרשרת מסדר שלישי‪.‬‬ ‫&‪e& 1 = e 2 x‬‬ ‫&‪e& 2 = e 3 x‬‬ ‫)‪(2.2.4‬‬ ‫‪e& 3 = u‬‬ ‫בחירת ‪ u‬באופן הבא‪,‬‬ ‫)‪(2.2.5‬‬ ‫‪σ = a1e1 + a 2e 2 + e3‬‬ ‫‪u =  − kh ( σ ) − a1e2 − a 2e3  x& , a1 , a 2 > 0‬‬ ‫שימוש במשפטים ‪ 2.1‬ו‪) 2.2 -‬כאשר ) ‪ ( v ( t ) = x& ( t‬וקיום התנאים ‪θ , φ < π 2 − ε , ε > 0‬‬ ‫מבטיחים יציבות אסימפטוטית של הראשית ב‪ .(2.2.4) -‬במקרה זה אות הבקרה שיש‬ ‫לספק ל‪ (1.2) -‬לצורך עקיבה אחרי המסלול ) ‪ yd = f ( x‬הוא‪:‬‬ ‫)‪(2.2.6‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪3 tan 2 φ tan θ‬‬ ‫&‪+ kh ( σ ) + a1e 2 + a 2e3  L cos2 φ cos3 θ x‬‬ ‫‪w =  f ′′′ ( x ) −‬‬ ‫‪2‬‬ ‫‪4‬‬ ‫‪L cos θ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪3 tan 2 φ tan θ‬‬ ‫‪= f ′′′ ( x ) −‬‬ ‫‪+ kh ( σ ) + a1e2 + a 2e3  L cos 2 φ cos 4 θ v‬‬ ‫‪2‬‬ ‫‪4‬‬ ‫‪L cos θ‬‬ ‫‪‬‬ ‫‪‬‬ ‫מתוך )‪ (2.2.1‬ברור כי התנאים הבאים מאפשרים יציבות מקומית של החוג הסגור‪:‬‬ ‫)‪(2.2.7‬‬ ‫‪θ < θmax = π 2 − ε1 , ε1 > 0‬‬ ‫‪φ < φmax = π 2 − ε 2 , ε 2 > 0‬‬ ‫ניתן למצוא תנאים מספיקים לקיום הדרישות ב‪ (2.2.7) -‬ע"י שימוש בחסם )‪(2.1.24‬‬ ‫ותכונות המסלול הרצוי‪ .‬נניח כי המסלול שאחריו רוצים לעקוב‪ ,‬המהירות הקווית של‬ ‫כלי הרכב וקבועי הבקר ‪ a1‬ו‪ a 2 -‬מקיימים‪:‬‬ ‫‪′ < ∞ , sup f ′′ ( x ) = fmax‬‬ ‫∞ < ‪′′‬‬ ‫‪sup f ′ ( x ) = fmax‬‬ ‫‪x‬‬ ‫)‪(2.2.8‬‬ ‫∞ < ‪, sup v ( t ) = vmax‬‬ ‫‪t‬‬ ‫‪x‬‬ ‫‪inf v ( t ) = vmin > 0‬‬ ‫‪t‬‬ ‫‪ 0‬‬ ‫‪1  ‬‬ ‫‪sup exp  ‬‬ ‫‪ t  = c3 , c 4 = c3 + 1‬‬ ‫‪a‬‬ ‫‪a‬‬ ‫‪−‬‬ ‫‪−‬‬ ‫‪t‬‬ ‫‪2 ‬‬ ‫‪ 1‬‬ ‫מתוך ההגדרה של ‪ e2‬ב‪ ,(2.2.1) -‬שימוש בחסם )‪ (2.1.24‬והצבה של )‪:(2.2.8‬‬ ‫‪29‬‬ ‫)‪(2.2.9‬‬ ‫) ‪a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0‬‬ ‫) ‪e1 ( 0‬‬ ‫‪+ c4‬‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪′ + c3‬‬ ‫‪tan θ = f ′ ( x ) − e2 ≤ f ′ ( x ) + e2 ≤ fmax‬‬ ‫כדי לקיים את הדרישה הראשונה ב‪ (2.2.7) -‬דרוש‪:‬‬ ‫)‪(2.2.10‬‬ ‫‪‬‬ ‫) ‪ < tan ( θmax‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫) ‪a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0‬‬ ‫)‪e ( 0‬‬ ‫‪′ + c3 1‬‬ ‫‪+ c4‬‬ ‫‪ fmax‬‬ ‫‪‬‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫מתוך ההגדרה של ‪ e3‬ב‪ ,(2.2.1) -‬שימוש בחסם )‪ (2.1.24‬והצבה של )‪:(2.2.8‬‬ ‫‪tan φ‬‬ ‫‪= f ′′ ( x ) − e3 cos3 θ ≤ f ′′ ( x ) + e3‬‬ ‫‪L‬‬ ‫)‪(2.2.11‬‬ ‫‪ c a2 + a2‬‬ ‫‪‬‬ ‫)‪e ( 0‬‬ ‫‪2‬‬ ‫‪′′ +  4 1‬‬ ‫‪≤ fmax‬‬ ‫‪+ 1 a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1‬‬ ‫) ‪e2 ( 0‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫‪‬‬ ‫כדי לקיים את הדרישה השנייה ב‪ (2.2.23) -‬דרוש‪:‬‬ ‫)‪(2.2.12‬‬ ‫‪ c a2 + a2‬‬ ‫‪‬‬ ‫)‪e ( 0‬‬ ‫) ‪tan ( φmax‬‬ ‫‪2‬‬ ‫‪′′ +  4 1‬‬ ‫‪fmax‬‬ ‫‪+ 1 a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1‬‬ ‫<‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫בגרפים ‪ 2.1-2.4‬מוצגות תוצאות של סימולציה אשר בוצעה עם מודל של כלי רכב בעל‬ ‫ארבעה גלגלים ובקר )‪ .(2.2.6‬תנאי ההתחלה של כלי הרכב בסימולציות הם‬ ‫‪ , x ( 0 ) = 0, y ( 0 ) = 4 [ m ] , θ ( 0 ) = 0, φ ( 0 ) = 0‬המרחק בין ציר הגלגלים הקדמי והאחורי‬ ‫]‪, L = 1 [m‬המסלול הרצוי ‪ , y ( x ) = sin ( x ) + 0.5x + 1‬המהירות הקווית של כלי הרכב‬ ‫] ‪ v = 1 [ m / s‬ונתוני הבקר הם ) ‪. k = 4, a1 = 2, a 2 = 2, h ( σ ) = tanh ( σ‬‬ ‫כדי להדגים את נחיצות הפונקציה הלא ליניארית )החסומה( ) ‪ h ( σ ) = tanh ( σ‬באות‬ ‫הבקרה )ראה הערה מספר ‪ 3‬בסעיף ‪ (2.1‬והאפשרות למנוע אותות בקרה גבוהים כאשר‬ ‫שגיאת המקום גדולה‪ ,‬מובאת כאן גם דוגמא עם תנאי התחלה רחוקים מהמסלול‬ ‫הרצוי באופן "קיצוני"‪ .‬בגרפים ‪ 2.5-2.8‬מוצגות תוצאות אשר התקבלו עם נתונים זהים‬ ‫לאלו ששימושו בסימולציות המוצגות בגרפים ‪ 2.1-2.4‬מלבד המקום ההתחלתי של כלי‬ ‫הרכב כאשר כאן ‪ . y ( 0 ) = 50 [ m] ⇒ e1 ( 0 ) = 49‬ניתן לראות כי למרות השגיאה‬ ‫ההתחלתית הגדולה במיקום כלי הרכב )באופן יחסי לתוצאות המוצגות בגרפים ‪(2.1-2.4‬‬ ‫אות הבקרה המקסימאלי כאן זהה לאות הבקרה המקסימאלי באיור ‪.2.2‬‬ ‫סימולציה נוספת המוצגת בגרפים ‪ 2.10-2.9‬מהווה דוגמה לפעולת הבקר כאשר כלי‬ ‫הרכב נוסע אחורה )כאן ) ‪ v ( t‬שלילי‪ ,‬ראה הערה מספר ‪ 1‬בסעיף ‪ .(2.1‬כל הנתונים‬ ‫‪30‬‬ ‫בסימולציה זו זהים לאלו ששימושו בסימולציות המוצגות בגרפים ‪ 2.1-2.4‬מלבד‬ ‫הנתונים הבאים‪ :‬המהירות הקווית כאן ] ‪ , v = −1 [ m / s‬קבועי הבקר הם ‪k = −4, a 2 = −2‬‬ ‫ותנאי ההתחלה ] ‪x ( 0 ) = 10 [ m‬‬ ‫כדוגמא מייצגת להרחבת השיטות המוצגות בעבודה זו ומיועדות לפתרון בעיה ה‪path -‬‬ ‫‪ following‬נציג כאן גם שיטה פשוטה המאפשרת הרחבה של בקר )‪ (2.2.6‬ומהווה פתרון‬ ‫לבעיית ה‪.trajectory tracking -‬‬ ‫נניח כי המסלול הרצוי )עם חוק זמן( נתונים באופן הבא‪:‬‬ ‫)‪(2.2.13‬‬ ‫∞ < ‪y d = f ( x d ) , x d = f ( t ) , f ( t ) ∈ R , 0 < η1 ≤ f& ( t ) ≤ η2‬‬ ‫בנוסף לשגיאות המוגדרות ב‪ (2.2.1) -‬נגדיר שגיאה נוספת‪:‬‬ ‫) ‪e0 ( t ) = f ( t ) − x ( t‬‬ ‫)‪(2.2.14‬‬ ‫ונניח כי נתון החסם הבא ‪:‬‬ ‫‪e0 ( 0 ) + ε < η3 , ε > 0‬‬ ‫)‪(2.2.15‬‬ ‫מגזירה של )‪ (2.2.14‬יתקבל‪:‬‬ ‫‪e& 0 = f& − x& = f& − cos θ v‬‬ ‫)‪(2.2.16‬‬ ‫נבחר אות בקרה ‪ v‬באופן הבא‪:‬‬ ‫‪1‬‬ ‫‪η3‬‬ ‫)‪(2.2.17‬‬ ‫<‪, 0<k‬‬ ‫&‪1 + ke0 ) f‬‬ ‫(‬ ‫=‪v‬‬ ‫‪cos θ‬‬ ‫מתוך הצבה של )‪ (2.2.17‬ב‪ (2.2.16) -‬מתקבל‪:‬‬ ‫) ‪e& 0 ( t ) = − ke0 ( t ) f& ( t‬‬ ‫)‪(2.2.18‬‬ ‫הוכחת היציבות כאן מבוססת על שימוש בפונקצית ליאפונוב באופן הבא‪:‬‬ ‫)‪(2.2.19‬‬ ‫‪& ( t ) = e ( t ) e& ( t ) = − ke2 ( t ) f& ( t ) < 0 , ∀e ( t ) ≠ 0‬‬ ‫‪⇒ V‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪1 2‬‬ ‫‪e0‬‬ ‫‪2‬‬ ‫= )‪V(t‬‬ ‫תפקיד החסם העליון של קבוע הבקר ‪ k‬הוא למנוע מהירות ) ‪ v ( t‬שלילית )או מהירות‬ ‫אפס( במקרה ש‪. x ( t ) > x d ( t ) -‬‬ ‫בגרפים ‪ 2.11-2.14‬מוצגות תוצאות של סימולציה אשר בוצעה עם בקר הגה )‪ (2.2.6‬ובקר‬ ‫מהירות )‪ .(2.2.17‬כל הנתונים של בקר )‪ (2.2.6‬והמסלול הרצוי זהים לאלו שהופיעו‬ ‫בסימולציות המתוארות בגרפים ‪ .2.1-2.4‬חוק הזמן של המסלול הרצוי נתון ע"י‬ ‫‪ x d ( t ) = t − 2‬וקבוע בקר המהירות הוא ‪. k = 0.4‬‬ ‫‪31‬‬ Trajectory trajectory desired trajectory 7 6 5 y [m] 4 3 2 1 0 0 1 2 3 4 5 x [m] 6 7 8 9 10 ((2.2.6) ‫ – מסלול כלי הרכב )סימולציה עם בקר‬2.1 ‫גרף‬ Control Inputs 2 v(t) 1.5 1 0.5 0 0 2 4 6 8 10 12 14 8 10 12 14 time [sec] 1 0.5 w(t) 0 -0.5 -1 -1.5 -2 0 2 4 6 time [sec] ((2.2.6) ‫ – אותות בקרה )סימולציה עם בקר‬2.2 ‫גרף‬ 32 ‫‪Vehicle Angles‬‬ ‫‪60‬‬ ‫‪40‬‬ ‫]‪θ(t) [deg‬‬ ‫‪20‬‬ ‫‪0‬‬ ‫‪-20‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-40‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫]‪φ (t) [deg‬‬ ‫‪0‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.3‬כיוון כלי הרכב וכיוון הגלגלים הקדמיים )סימולציה עם בקר )‪((2.2.6‬‬ ‫‪Tracking Errors‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫)‪e1(t‬‬ ‫‪-1‬‬ ‫‪-2‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-3‬‬ ‫]‪time [sec‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫)‪e2(t‬‬ ‫‪0‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫]‪time [sec‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫)‪e3(t‬‬ ‫‪-0.5‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.4‬שגיאות ביחס למסלול הרצוי )סימולציה עם בקר )‪((2.2.6‬‬ ‫‪33‬‬ ‫‪Trajectory‬‬ ‫‪50‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪45‬‬ ‫‪40‬‬ ‫‪35‬‬ ‫‪30‬‬ ‫]‪y [m‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪40‬‬ ‫‪50‬‬ ‫‪20‬‬ ‫‪30‬‬ ‫‪0‬‬ ‫‪10‬‬ ‫]‪x [m‬‬ ‫גרף ‪ – 2.5‬מסלול כלי הרכב )סימולציה עם בקר )‪ (2.2.6‬ותנאי התחלה "קיצוניים"(‬ ‫‪Control Inputs‬‬ ‫‪2‬‬ ‫‪1.5‬‬ ‫)‪v(t‬‬ ‫‪1‬‬ ‫‪0.5‬‬ ‫‪80‬‬ ‫‪70‬‬ ‫‪60‬‬ ‫‪50‬‬ ‫‪40‬‬ ‫]‪time [sec‬‬ ‫‪30‬‬ ‫‪20‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪80‬‬ ‫‪70‬‬ ‫‪60‬‬ ‫‪50‬‬ ‫‪40‬‬ ‫]‪time [sec‬‬ ‫‪30‬‬ ‫‪20‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫)‪w(t‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫‪-2‬‬ ‫גרף ‪ – 2.6‬אותות בקרה )סימולציה עם בקר )‪ (2.2.6‬ותנאי התחלה "קיצוניים"(‬ ‫‪34‬‬ ‫‪Tracking Errors‬‬ ‫‪20‬‬ ‫‪0‬‬ ‫)‪e1(t‬‬ ‫‪-20‬‬ ‫‪-40‬‬ ‫‪80‬‬ ‫‪70‬‬ ‫‪60‬‬ ‫‪50‬‬ ‫‪40‬‬ ‫]‪time [sec‬‬ ‫‪30‬‬ ‫‪20‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪80‬‬ ‫‪70‬‬ ‫‪60‬‬ ‫‪50‬‬ ‫‪40‬‬ ‫]‪time [sec‬‬ ‫‪30‬‬ ‫‪20‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪80‬‬ ‫‪70‬‬ ‫‪60‬‬ ‫‪50‬‬ ‫‪40‬‬ ‫]‪time [sec‬‬ ‫‪30‬‬ ‫‪20‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪-60‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫)‪e2(t‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫)‪e3(t‬‬ ‫‪-0.5‬‬ ‫‪-1‬‬ ‫‪-1.5‬‬ ‫גרף ‪ – 2.7‬שגיאות )סימולציה עם בקר )‪ (2.2.6‬ותנאי התחלה "קיצוניים"(‬ ‫‪0.4‬‬ ‫‪0.2‬‬ ‫‪0‬‬ ‫‪-0.2‬‬ ‫))‪tanh(σ (t‬‬ ‫‪-0.4‬‬ ‫‪-0.6‬‬ ‫‪-0.8‬‬ ‫‪80‬‬ ‫‪70‬‬ ‫‪60‬‬ ‫‪50‬‬ ‫‪40‬‬ ‫]‪time [sec‬‬ ‫‪30‬‬ ‫‪20‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫גרף ‪ – 2.8‬הפונקציה ) ) ‪) h ( σ ( t‬סימולציה עם בקר )‪ (2.2.6‬ותנאי התחלה "קיצוניים"(‬ ‫‪35‬‬ ‫‪Trajectory‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪7‬‬ ‫‪6‬‬ ‫‪5‬‬ ‫‪4‬‬ ‫]‪y [m‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪9‬‬ ‫‪7‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪5‬‬ ‫]‪x [m‬‬ ‫‪3‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫גרף ‪ – 2.9‬מסלול כלי הרכב )סימולציה עם בקר )‪ (2.2.6‬ומהירות ) ‪ v ( t‬שלילית(‬ ‫‪Control Inputs‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫)‪v(t‬‬ ‫‪-1‬‬ ‫‪-1.5‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫]‪time [sec‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫)‪w(t‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.10‬אותות בקרה )סימולציה עם בקר )‪ (2.2.6‬ומהירות ) ‪ v ( t‬שלילית(‬ ‫‪36‬‬ Trajectory trajectory desired trajectory 7 6 5 y [m] 4 3 2 1 0 0 1 2 3 4 5 x [m] 6 7 8 9 10 ,(2.2.12) -‫( ו‬2.2.6) ‫ – מסלול כלי הרכב )סימולציה עם בקרים‬2.11 ‫גרף‬ (Trajectory tracking Control Inputs 2 v(t) 1.5 1 0.5 0 0 2 4 6 time [sec] 8 10 12 0 2 4 6 time [sec] 8 10 12 1 w(t) 0.5 0 -0.5 -1 (Trajectory tracking ,(2.2.12) -‫( ו‬2.2.6) ‫ – אותות בקרה )סימולציה עם בקרים‬2.12 ‫גרף‬ 37 x(t) vs. xd(t) 12 actual x(t) desired x(t) 10 8 x [m] 6 4 2 0 -2 0 2 4 6 time [sec] 8 10 12 ,(2.2.12) -‫( ו‬2.2.6) ‫ )סימולציה עם בקרים‬x ( t ) ‫ – משתנה המצב‬2.13 ‫גרף‬ (Trajectory tracking Tracking Errors e0(t) 0 -1 -2 0 2 4 6 0 2 4 6 0 2 4 6 0 2 4 6 e1(t) 2 10 12 8 10 12 8 10 12 8 10 12 -2 2 e2(t) 8 0 -4 time [sec] 1 0 -1 0.5 e3(t) time [sec] time [sec] 0 -0.5 -1 time [sec] (Trajectory tracking ,(2.2.12) -‫( ו‬2.2.6) ‫ – שגיאות )סימולציה עם בקרים‬2.14 ‫גרף‬ 38 ‫הבקר )‪ (2.2.6‬מתאים לשימוש רק כאשר כיוון כלי הרכב מקיים ‪, θ < π 2 − ε , ε > 0‬‬ ‫חסרון נוסף של בקר זה )כפי שיובהר בהמשך( קשור בעובדה כי הוא מבוסס על מודל‬ ‫שרשרת אשר בו ‪ . v ( t ) = vcosθ‬כדי להימנע מחסרונות אלה מוצעת בעבודה זו‬ ‫הטרנספורמציה הבאה‪:‬‬ ‫‪e1 = f ( x ) − y‬‬ ‫‪e2 = f ′ ( x ) cos θ − sin θ‬‬ ‫)‪(2.2.20‬‬ ‫‪tan φ‬‬ ‫) ‪( f ′ ( x ) sin θ + cos θ‬‬ ‫‪L‬‬ ‫‪e3 = f ′′ ( x ) cos 2 θ −‬‬ ‫כאשר גם כאן‪ e1 :‬שגיאת מקום‪ e 2 ,‬שגיאת כיוון ו‪ e 3 -‬שגיאה בכיוון הגלגלים‬ ‫הקדמיים של כלי הרכב‪ .‬ראוי להדגיש כי המצב הסינגולארי היחיד הקיים ב‪(2.2.15) -‬‬ ‫הינו ‪ φ ≠ π 2‬והוא תולדה של המבנה המכאני של כלי הרכב‪.‬‬ ‫נגדיר את כיוון המסלול הרצוי ע"י הזווית ) ‪ θr ( x‬כלומר‪:‬‬ ‫) ‪sin θr ( x‬‬ ‫) ‪cos θr ( x‬‬ ‫)‪(2.2.21‬‬ ‫= ) ‪f ′ ( x ) = tan θr ( x‬‬ ‫נראה בהמשך כי תכנון בקר המבוסס על )‪ (2.2.20‬מצריך ‪ , θr − θ < π 2 − ε , ε > 0‬דרישה זו‬ ‫נובעת מסינגולאריות של אות הבקרה כאשר כלי הרכב ניצב למסלול )בנקודה ‪ .(x‬מגבלה זו‬ ‫נתפשת באופן יותר טבעי מאשר המגבלה ‪ θ < π 2 − ε , ε > 0‬המאפיינת את בקר )‪(2.2.6‬‬ ‫מכיוון שכאשר כלי הרכב ניצב למסלול הרצוי "לא ברור" האם יש לפנות ימינה או שמאלה‪.‬‬ ‫מגזירת )‪ (2.2.20‬והצבה של מודל כלי הרכב )‪ (1.2‬תתקבל המערכת הבאה‪:‬‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = e3 v‬‬ ‫)‪(2.2.22‬‬ ‫‪‬‬ ‫‪f ′′ ( x ) cos θ sin θ tan φ f ′ ( x ) cos θ tan 2 φ sin θ tan 2 φ ‬‬ ‫‪e& 3 =  f ′′′ ( x ) cos3 θ − 3‬‬ ‫‪−‬‬ ‫‪+‬‬ ‫‪ v‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪‬‬ ‫‪L‬‬ ‫‪L‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ cos θ + f ′ ( x ) sin θ ‬‬ ‫‪−‬‬ ‫‪w‬‬ ‫‪2‬‬ ‫‪cos‬‬ ‫‪L‬‬ ‫‪φ‬‬ ‫‪‬‬ ‫‪‬‬ ‫נבחר אות בקרה מהצורה‪:‬‬ ‫‪39‬‬ ‫)‪(2.2.23‬‬ ‫‪‬‬ ‫‪f ′′ ( x ) cos θ sin θ tan φ f ′ ( x ) cos θ tan 2 φ‬‬ ‫‪3‬‬ ‫‪−‬‬ ‫‪w =  f ′′′ ( x ) cos θ − 3‬‬ ‫‪L‬‬ ‫‪L2‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪sin θ tan 2 φ ‬‬ ‫‪L cos 2 φ‬‬ ‫‪+‬‬ ‫‪ v − u‬‬ ‫‪L2‬‬ ‫‪‬‬ ‫‪ cos θ + f ′ ( x ) sin θ‬‬ ‫כאשר מ‪ (2.2.23) -‬ברור כי יש להימנע ממצב בו ‪ . ( cos θ + f ′ ( x ) sin θ ) = 0‬דרישה זו‬ ‫גוררת ‪ ( cos θ + f ′ ( x ) sin θ ) > 0‬כאשר יש לעקוב אחרי המסלול הרצוי כך ש‪cos θr > 0 :‬‬ ‫)כלומר נסיעה בכיוון ציר ‪ ,(x‬ו‪ ( cos θ + f ′ ( x ) sin θ ) < 0 -‬כאשר יש לעקוב אחרי המסלול‬ ‫הרצוי כך ש‪) cos θr < 0 :‬כלומר נסיעה בכיוון השלילי של ציר ‪(x‬‬ ‫במקרה הראשון‪.‬‬ ‫)‪(2.2.24‬‬ ‫‪sin θr‬‬ ‫‪sin θ > 0 , cos θr > 0‬‬ ‫‪cos θr‬‬ ‫‪cos θ + f ′ ( x ) sin θ = cos θ +‬‬ ‫‪⇒ cos θr cos θ + sin θr sin θ = cos ( θr − θ ) > 0‬‬ ‫‪θr − θ < π 2 − ε , ε > 0‬‬ ‫⇒‬ ‫תוצאה דומה ניתן לקבל גם במקרה השני‪.‬‬ ‫מתוך הצבה של הבקר )‪ (2.2.23‬במודל )‪ (2.2.22‬מתקבל מודל בצורת שרשרת מסדר‬ ‫שלישי‪.‬‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = e3 v‬‬ ‫)‪(2.2.25‬‬ ‫‪e& 3 = u‬‬ ‫כאשר כאן ‪. v ( t ) = v‬‬ ‫בחירת ‪ u‬באופן הבא‪,‬‬ ‫)‪(2.2.26‬‬ ‫‪σ = a1e1 + a 2e2 + e3‬‬ ‫‪u =  − kh ( σ ) − a1e2 − a 2e3  v , a1 , a 2 > 0‬‬ ‫שימוש במשפטים ‪ 2.1‬ו‪) 2.2 -‬כאשר ) ‪ ( v ( t ) = v ( t‬וקיום התנאים ‪θr − θ , φ < π 2 − ε , ε > 0‬‬ ‫מבטיחים יציבות אסימפטוטית של הראשית ב‪.(2.2.25) -‬‬ ‫במקרה זה אות הבקרה שיש לספק ל‪ (1.2) -‬לצורך עקיבה אחרי המסלול ) ‪yd = f ( x‬‬ ‫הוא‪:‬‬ ‫‪40‬‬ ‫)‪(2.2.27‬‬ ‫‪‬‬ ‫‪f ′′ ( x ) cos θ sin θ tan φ f ′ ( x ) cos θ tan 2 φ‬‬ ‫‪−‬‬ ‫‪w =  f ′′′ ( x ) cos3 θ − 3‬‬ ‫‪‬‬ ‫‪L‬‬ ‫‪L2‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪L cos 2 φ‬‬ ‫‪sin θ tan 2 φ‬‬ ‫‪kh‬‬ ‫‪a‬‬ ‫‪e‬‬ ‫‪a‬‬ ‫‪e‬‬ ‫‪v‬‬ ‫‪+‬‬ ‫‪+‬‬ ‫‪σ‬‬ ‫‪+‬‬ ‫‪+‬‬ ‫(‬ ‫)‬ ‫‪‬‬ ‫‪1 2‬‬ ‫‪2 3‬‬ ‫‪L2‬‬ ‫‪ cos θ + f ′ ( x ) sin θ‬‬ ‫מתוך )‪ (2.2.20‬ו‪ (2.2.24) -‬ברור כי התנאים הבאים מאפשרים יציבות מקומית של החוג‬ ‫הסגור‪:‬‬ ‫)‪(2.2.28‬‬ ‫‪θr − θ < θmax = π 2 − ε1 , ε1 > 0‬‬ ‫‪φ < φmax = π 2 − ε 2 , ε 2 > 0‬‬ ‫ניתן למצוא תנאים מספיקים לקיום הדרישות ב‪ (2.3.28) -‬ע"י שימוש בחסם )‪(2.1.24‬‬ ‫ותכונות המסלול הרצוי‪ .‬נניח כי המסלול שאחריו רוצים לעקוב‪ ,‬המהירות הקווית של‬ ‫כלי הרכב וקבועי הבקר ‪ a1‬ו‪ a 2 -‬מקיימים‪:‬‬ ‫‪′ < ∞ , sup f ′′ ( x ) = fmax‬‬ ‫∞ < ‪′′‬‬ ‫‪sup f ′ ( x ) = fmax‬‬ ‫‪x‬‬ ‫)‪(2.2.29‬‬ ‫∞ < ‪, sup v ( t ) = vmax‬‬ ‫‪t‬‬ ‫‪x‬‬ ‫‪inf v ( t ) = vmin > 0‬‬ ‫‪t‬‬ ‫‪ 0‬‬ ‫‪1  ‬‬ ‫‪sup exp  ‬‬ ‫‪ t  = c3 , c 4 = c3 + 1‬‬ ‫‪t‬‬ ‫‪  − a1 − a 2  ‬‬ ‫במודל השרשרת )‪ v ( t ) = v ( t ) (2.2.25‬לכן‬ ‫‪, η2 = vmax‬‬ ‫)‪(2.2.30‬‬ ‫‪η1 = vmin‬‬ ‫מתוך ההגדרה של ‪ e2‬ב‪ ,(2.2.15) -‬שימוש בחסם )‪ (2.1.24‬והצבה של )‪:(2.2.24‬‬ ‫) ‪sin θr cos θ − cos θr sin θ sin ( θr − θ‬‬ ‫=‬ ‫‪cos θr‬‬ ‫‪cos θr‬‬ ‫)‪(2.2.31‬‬ ‫= ‪e2 = f ′ ( x ) cos θ − sin θ‬‬ ‫) ‪a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0‬‬ ‫)‪e (0‬‬ ‫) ‪sin ( θr − θ‬‬ ‫‪< c3 1‬‬ ‫‪+ c4‬‬ ‫) ‪e2 ( 0‬‬ ‫‪cos θr‬‬ ‫‪a1‬‬ ‫⇒‬ ‫‪sin ( θr − θ ) < c3‬‬ ‫⇒‬ ‫) ‪a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0‬‬ ‫) ‪e1 ( 0‬‬ ‫‪+ c4‬‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫כדי לקיים את הדרישה הראשונה ב‪ (2.2.28) -‬דרוש‪:‬‬ ‫)‪(2.2.32‬‬ ‫‪‬‬ ‫) ‪ < sin ( θmax‬‬ ‫‪‬‬ ‫‪‬‬ ‫) ‪ e1 ( 0‬‬ ‫) ‪a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0‬‬ ‫‪+ c4‬‬ ‫‪ c3‬‬ ‫) ‪ e2 ( 0‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫מתוך ההגדרה של ‪ e3‬ב‪ ,(2.2.20) -‬שימוש בחסם )‪ (2.1.24‬והצבה של )‪:(2.2.29‬‬ ‫‪41‬‬ ‫‪‬‬ ‫‪tan φ  sin θr‬‬ ‫‪sin θ + cos θ ‬‬ ‫‪‬‬ ‫‪L  cos θr‬‬ ‫‪‬‬ ‫‪tan φ  cos ( θr − θ ) ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪L  cos θr ‬‬ ‫‪e3 = f ′′ ( x ) cos 2 θ −‬‬ ‫‪= f ′′ ( x ) cos 2 θ −‬‬ ‫⇒‬ ‫)‪(2.2.33‬‬ ‫) ‪tan φ cos ( θr − θ‬‬ ‫‪= f ′′ ( x ) cos 2 θ − e3 cos θr ≤ f ′′ ( x ) + e3‬‬ ‫‪L‬‬ ‫)‬ ‫(‬ ‫‪ c a2 + a2‬‬ ‫‪‬‬ ‫)‪e ( 0‬‬ ‫) ‪tan φ cos ( θr − θ‬‬ ‫‪2‬‬ ‫‪′′ +  4 1‬‬ ‫‪≤ fmax‬‬ ‫‪+ 1 a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1‬‬ ‫) ‪e2 ( 0‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪L‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫‪‬‬ ‫אם )‪ (2.2.32‬מתקיים אז ) ‪ , cos ( θr − θ ) > cos ( θmax‬במקרה זה כדי לקיים את הדרישה‬ ‫השנייה ב‪ (2.2.28) -‬דרוש‪:‬‬ ‫)‪(2.2.34‬‬ ‫‪ c a2 + a2‬‬ ‫‪‬‬ ‫) ‪tan ( φmax ) cos ( θmax‬‬ ‫)‪e ( 0‬‬ ‫‪2‬‬ ‫‪′′ +  4 1‬‬ ‫‪fmax‬‬ ‫‪+ 1 a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1‬‬ ‫<‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫)קיום הדרישה השנייה ב‪ (2.2.28) -‬מחייב גם קיום הדרישה הראשונה(‪.‬‬ ‫בגרפים ‪ 2.15-2.18‬מוצגות תוצאות של סימולציה אשר בוצעה עם מודל של כלי רכב‬ ‫בעל ארבעה גלגלים ובקר )‪ .(2.2.27‬תנאי ההתחלה של כלי הרכב בסימולציה הם‬ ‫‪ , x ( 0 ) = 1 [ m] , y ( 0 ) = 4 [ m] , θ ( 0 ) = 120o , φ ( 0 ) = 0‬המרחק בין ציר הגלגלים הקדמי‬ ‫והאחורי ]‪, L = 1 [m‬המסלול הרצוי ‪ , y ( x ) = sin ( x ) + 0.5x + 1‬המהירות הקווית‬ ‫] ‪ v = 1 [ m / s‬ונתוני הבקר הם ) ‪. k = 4, a1 = 2, a 2 = 4, h ( σ ) = tanh ( σ‬‬ ‫ראוי להדגיש כאן כי תנאי התחלה ‪ θ ( 0 ) = 120o‬אינו אפשרי כאשר משתמשים בבקר‬ ‫)‪ (2.2.6‬מכיוון שבמקרה זה התכנסות אל המסלול הרצוי מצריכה מעבר דרך המצב‬ ‫הסינגולארי ‪ θ ( t ) = 90o‬הקיים בבקר )‪.(2.2.6‬‬ ‫הערות לגבי בקר )‪:(2.2.27‬‬ ‫‪ .1‬מתוך תוצאות הסימולציה עולה כי‬ ‫‪, φmax = 63o‬‬ ‫‪42‬‬ ‫‪θmax = 73.8o‬‬ ‫לעומת זאת ברור כי תנאי ההתחלה של כלי הרכב אינם מקיימים את החסמים )‪(2.2.32‬‬ ‫ו‪) (2.2.34) -‬לדוגמא‪ :‬כדי לקיים את חסם )‪ (2.2.32‬דרוש ‪ .( e1 ( 0 ) < 1, e2 ( 0 ) < 1,‬מכאן ברור‬ ‫כי חסמים אלה מאופיינים בשמרנות ייתר ולא תמיד יתאימו לצרכים מעשיים )תפקידם‬ ‫העיקרי הוא לאפשר הוכחה של יציבות מקומית(‪ .‬בכלי רכב בעלי ארבעה גלגלים‪ ,‬עקב‬ ‫אילוצים מכאניים‪ ,‬זווית ההגה מוגבלת )בדרך כלל הזווית המקסימלית ‪ .( 40o − 50o‬סביר‬ ‫שאם יעשה שימוש בחסמים )‪ (2.2.32‬ו‪ (2.2.34) -‬כדי להבטיח שזווית ההגה תישאר‬ ‫בתחום המוכתב ע"י האילוצים המכאניים נקבל )מלבד האילוצים הברורים על המסלול‬ ‫הרצוי( כי תנאי ההתחלה של כלי הרכב חייבים להיות קרובים מאוד למסלול הרצוי‪.‬‬ ‫לעומת זאת סימולציות וניסויים רבים שבצעתי מראים כי תוספת לבקר המגבילה את‬ ‫זווית ההגה באופן הבא‪:‬‬ ‫)‪(2.2.35‬‬ ‫‪φ ( t ) ≥ φmax , φ& ( t ) > 0‬‬ ‫‪φ ( t ) ≤ φ , φ& ( t ) < 0‬‬ ‫‪‬‬ ‫‪ ⇒ w (t) = 0‬‬ ‫‪‬‬ ‫‪min‬‬ ‫‪otherwise w ( t ) = unchange‬‬ ‫מאפשרת התכנסות למסלול )או לסביבה קרובה‪ ,‬אם נסיעה על המסלול הרצוי מצריכה‬ ‫זוויות הגה שאינן מקימות את האילוצים המכאניים( ופגיעה קטנה מאוד בביצועי‬ ‫הבקר‪ .‬תוצאות סימולציה אשר בוצעה עם בקר )‪ ,(2.2.27‬נתונים זהים לנתונים‬ ‫בסימולציה הקודמת ותוספת מנגנון המגביל את זווית ההגה כך ש ‪ φ ( t ) < 35o‬מוצגים‬ ‫בגרפים ‪.2.19-2.22‬‬ ‫‪ .2‬אם ‪ θr − θ > π 2 + ε , ε > 0‬אז כלי הרכב עדיין יתכנס למסלול אבל בכיוון ההפוך‬ ‫)כלומר כך ש‪ x ( t ) -‬קטן(‪ .‬דוגמא לתוצאות של סימולציה כזאת כאשר ‪ θ ( 0 ) = 180o‬ו‪-‬‬ ‫‪ x ( 0 ) = 5‬מוצגות בגרפים ‪) 2.23-2.26‬כל שאר הנתונים זהים לאלו ששימשו בסימולציות‬ ‫המוצגות בגרפים ‪ .(2.15-2.18‬ראוי להדגיש כי מדובר במהירות ) ‪ v ( t‬חיובית‪ .‬לצורך‬ ‫השוואה נוחה של גרף ‪ 2.23‬עם גרף ‪) 2.27‬אשר תכליתו נתונה בהערה מספר ‪ (3‬מוצג גם‬ ‫קטע המסלול ]‪) x = 5 − 15 [ m‬למרות שבסימולציה זו קטע מסלול זה אינו חופף‬ ‫למסלול הנסיעה של כלי הרכב(‬ ‫‪ .3‬ע"י הפעלת אות הבקרה )‪ (2.2.27‬כאשר זווית כלי הרכב ) ‪ θ ( t‬מוחלפת באות ) ‪θ ( t‬‬ ‫המוגדר באופן הבא‪:‬‬ ‫‪43‬‬ ‫)‪(2.2.36‬‬ ‫‪ θ( t ),‬‬ ‫) ‪cos ( θ ( t ) − θr ( t ) ) ≥ cos ( θmax‬‬ ‫‪‬‬ ‫‪θ ( t ) = θr ( t ) + θmax , cos ( θ ( t ) − θr ( t ) ) < cos ( θmax ) , sin ( θ ( t ) − θr ( t ) ) ≥ 0‬‬ ‫‪‬‬ ‫‪ θr ( t ) − θmax , cos ( θ ( t ) − θr ( t ) ) < cos ( θmax ) , sin ( θ ( t ) − θr ( t ) ) <0‬‬ ‫‪θmax = π 2 − ε1 , ε1 > 0‬‬ ‫ניתן לאלץ את כלי הרכב להתכנס למסלול בכיוון ציר ‪) x‬כלומר כך ש‪ x ( t ) -‬גדל( מכל‬ ‫תנאי התחלה ) ‪ . θ ( 0‬ניתן לתאר רעיון זה‪ ,‬באופן הבא‪ :‬כאשר השגיאה בכיוון כלי הרכב‬ ‫גדולה מדי אז "לא מספקים לבקר את הכיוון האמיתי של כלי הרכב"‪ ,‬מתוך סימולציות‬ ‫רבות עולה כי רעיון זה אינו פוגם ביכולת של הבקר להפעיל את כלי הרכב כך ששגיאות‬ ‫העקיבה יקטנו‪ ,‬כאשר השגיאה בכיוון כלי הרכב קטנה מספיק אז הבקר מופעל באופן‬ ‫רגיל וביצועיו מגובים ברקע התיאורטי שהוצג כאן‪ .‬תוצאות סימולציה של בקר )‪(2.2.27‬‬ ‫עם מנגנון כזה כאשר ‪ θ ( 0 ) = 180o‬ו‪ θmax = 70o -‬מוצגים בגרפים ‪) 2.27-2.30‬כל שאר‬ ‫הנתונים זהים לאלו ששימשו בסימולציות המוצגות בגרפים ‪.(2.15-2.18‬‬ ‫‪ .4‬הבדל חשוב בין הבקר כאן )‪ (2.2.27‬ובין הבקר שהוצג קודם )‪ (2.2.6‬קשור בקצב‬ ‫ההתכנסות של כלי הרכב אל המסלול הרצוי‪ .‬מתוך )‪ (2.1.9-10‬ברור כי קצב הדעיכה של‬ ‫ווקטור השגיאות ) ‪ e ( t‬במודל השרשרת תלוי בין היתר בחסם התחתון של הפונקציה‬ ‫) ‪ , v ( t‬ככול שחסם זה נמוך יותר כלי הרכב מתכנס לאט יותר‪ .‬בבקר )‪(2.2.27‬‬ ‫) ‪ v ( t ) = v ( t‬ואילו בבקר )‪ , v ( t ) = x& ( t ) = vcos ( θ ) (2.2.6‬העובדה כי קצב ההתכנסות‬ ‫בבקר )‪ (2.2.6‬תלוי גם בכיוון הנסיעה של כלי הרכב עשויה להאט מאוד את ההתכנסות‬ ‫אל המסלול הרצוי כאשר כיוון כלי הרכב מתקרב ל‪ . ± π 2 -‬דוגמא להשוואה בין שני‬ ‫הבקרים מוצגת בגרף ‪ 2.31‬כאשר נתוני הבקר הם ) ‪, k = 4, a1 = 2, a 2 = 2, h ( σ ) = tanh ( σ‬‬ ‫המסלול הרצוי ‪ , y ( x ) = 5x − 3‬תנאי התחלה ‪x ( 0 ) = 0, y ( 0 ) = 4 [ m ] , θ ( 0 ) = 80o , φ ( 0 ) = 0‬‬ ‫והמהירות הקווית ] ‪ . v = 1 [ m / s‬למרות העובדה כי כל הנתונים עבור שני הבקרים זהים‪,‬‬ ‫הבקר )‪ (2.2.27‬מאפשר כאן קצב התכנסות מהיר יותר באופן משמעותי‪ .‬מסיבה זו בכל‬ ‫הבקרים הנוספים אשר פותחו במסגרת עבודה זו‪ ,‬נעשה שימוש )במידת האפשר(‬ ‫בטרנספורמציות אשר מובילות למודל שרשרת שבו ) ‪ , v ( t ) = v ( t‬כלומר הפונקציה‬ ‫) ‪ v ( t‬הינה המהירות הקווית של כלי הרכב )ואינה תלויה במצב כלי הרכב(‪.‬‬ ‫‪44‬‬ Trajectory 8 trajectory desired trajectory 7 6 y [m] 5 4 3 2 1 0 0 1 2 3 4 5 x [m] 6 7 8 9 10 ((2.2.27) ‫ – מסלול כלי הרכב )סימולציה עם בקר‬2.15 ‫גרף‬ Control Inputs 2 v(t) 1.5 1 0.5 0 0 2 4 6 8 10 12 14 8 10 12 14 time [sec] w(t) 5 0 -5 0 2 4 6 time [sec] ((2.2.27) ‫ – אותות בקרה )סימולציה עם בקר‬2.16 ‫גרף‬ 45 Vehicle Angles 200 θ(t) [deg] 100 0 -100 0 2 4 6 8 10 12 14 8 10 12 14 8 10 12 14 time [sec] (θr-θ) [deg] 50 0 -50 0 2 4 6 time [sec] φ (t) [deg] 50 0 -50 0 2 4 6 time [sec] ((2.2.27) ‫ – כיוון כלי הרכב וכיוון הגלגלים הקדמיים )סימולציה עם בקר‬2.17 ‫גרף‬ Tracking Errors 0 e1(t) -1 -2 -3 0 2 4 6 8 10 12 14 8 10 12 14 8 10 12 14 time [sec] 1 e2(t) 0 -1 -2 0 2 4 6 time [sec] 1.5 e3(t) 1 0.5 0 -0.5 0 2 4 6 time [sec] ((2.2.27) ‫ – שגיאות ביחס למסלול הרצוי )סימולציה עם בקר‬2.18 ‫גרף‬ 46 ‫‪Trajectory‬‬ ‫‪8‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪7‬‬ ‫‪6‬‬ ‫‪5‬‬ ‫]‪y [m‬‬ ‫‪4‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪10‬‬ ‫‪9‬‬ ‫‪7‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪5‬‬ ‫]‪x [m‬‬ ‫‪3‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫גרף ‪ – 2.19‬מסלול כלי הרכב )סימולציה עם בקר )‪ (2.2.27‬ומנגנון )‪ (2.2.35‬המגביל את‬ ‫זווית ההגה(‬ ‫‪Control Inputs‬‬ ‫‪2‬‬ ‫‪1.5‬‬ ‫)‪v(t‬‬ ‫‪1‬‬ ‫‪0.5‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫)‪w(t‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫‪-2‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-3‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.20‬אותות בקרה )סימולציה עם בקר )‪ (2.2.27‬ומנגנון )‪ (2.2.35‬המגביל את זווית‬ ‫ההגה(‬ ‫‪47‬‬ ‫‪Vehicle Angles‬‬ ‫‪200‬‬ ‫]‪θ(t) [deg‬‬ ‫‪100‬‬ ‫‪0‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-100‬‬ ‫]‪time [sec‬‬ ‫‪100‬‬ ‫]‪(θr-θ) [deg‬‬ ‫‪50‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-100‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫]‪φ (t) [deg‬‬ ‫‪0‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.21‬כיוון כלי הרכב וכיוון הגלגלים הקדמיים )סימולציה עם בקר )‪ (2.2.27‬ומנגנון‬ ‫)‪ (2.2.35‬המגביל את זווית ההגה(‬ ‫‪Tracking Errors‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫)‪e1(t‬‬ ‫‪-2‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪-3‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫)‪e2(t‬‬ ‫‪-1‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫]‪time [sec‬‬ ‫‪1.5‬‬ ‫‪1‬‬ ‫)‪e3(t‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.22‬שגיאות ביחס למסלול הרצוי )סימולציה עם בקר )‪ (2.2.27‬ומנגנון )‪(2.2.35‬‬ ‫המגביל את זווית ההגה(‬ ‫‪48‬‬ Trajectory 12 trajectory desired trajectory 10 8 y [m] 6 4 2 0 -2 -5 0 5 x [m] 10 15 ( θ ( 0 ) = 180o ‫( ותנאי התחלה‬2.2.27) ‫ – מסלול כלי הרכב )סימולציה עם בקר‬2.23 ‫גרף‬ Control Inputs 2 v(t) 1.5 1 0.5 0 0 2 4 6 8 10 12 14 8 10 12 14 time [sec] 1.5 1 w(t) 0.5 0 -0.5 -1 0 2 4 6 time [sec] ( θ ( 0 ) = 180o ‫( ותנאי התחלה‬2.2.27) ‫ – אותות בקרה )סימולציה עם בקר‬2.24 ‫גרף‬ 49 Vehicle Angles θ(t) [deg] 250 200 150 0 2 4 6 40 (θr-θ) [deg] 8 10 12 14 8 10 12 14 8 10 12 14 time [sec] 20 0 -20 -40 0 2 4 6 time [sec] φ (t) [deg] 50 0 -50 0 2 4 6 time [sec] ‫( ותנאי‬2.2.27) ‫ – כיוון כלי הרכב וכיוון הגלגלים הקדמיים )סימולציה עם בקר‬2.25 ‫גרף‬ ( θ ( 0 ) = 180o ‫התחלה‬ Tracking Errors 0 e1(t) -0.5 -1 -1.5 -2 0 2 4 6 8 10 12 14 8 10 12 14 8 10 12 14 time [sec] 1 e2(t) 0.5 0 -0.5 -1 0 2 4 6 time [sec] 1.5 e3(t) 1 0.5 0 -0.5 0 2 4 6 time [sec] ( θ ( 0 ) = 180o ‫( ותנאי התחלה‬2.2.27) ‫ – שגיאות )סימולציה עם בקר‬2.26 ‫גרף‬ 50 ‫‪Trajectory‬‬ ‫‪12‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫]‪y [m‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪5‬‬ ‫]‪x [m‬‬ ‫‪-5‬‬ ‫גרף ‪ – 2.27‬מסלול כלי הרכב )סימולציה עם בקר )‪ ,(2.2.27‬תנאי התחלה ‪θ ( 0 ) = 180o‬‬ ‫והחלפת ) ‪ θ ( t‬ב‪ θ ( t ) -‬לפי‪((2.2.36) -‬‬ ‫‪Control Inputs‬‬ ‫‪2‬‬ ‫‪1.5‬‬ ‫)‪v(t‬‬ ‫‪1‬‬ ‫‪0.5‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪5‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫)‪w(t‬‬ ‫‪0‬‬ ‫‪-5‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.28‬אותות בקרה )סימולציה עם בקר )‪ ,(2.2.27‬תנאי התחלה ‪ θ ( 0 ) = 180o‬ו‬ ‫והחלפת ) ‪ θ ( t‬ב‪ θ ( t ) -‬לפי‪((2.2.36) -‬‬ ‫‪51‬‬ ‫‪Vehicle Angles‬‬ ‫‪200‬‬ ‫‪0‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫]‪θ(t) [deg‬‬ ‫‪100‬‬ ‫‪-100‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫]‪(θr-θ) [deg‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫‪-100‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-150‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫]‪φ (t) [deg‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-100‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.29‬כיוון כלי הרכב וכיוון הגלגלים הקדמיים )סימולציה עם בקר )‪ ,(2.2.27‬תנאי‬ ‫התחלה ‪ θ ( 0 ) = 180o‬ו והחלפת ) ‪ θ ( t‬ב‪ θ ( t ) -‬לפי‪((2.2.36) -‬‬ ‫‪Tracking Errors‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫)‪e1(t‬‬ ‫‪-2‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-3‬‬ ‫]‪time [sec‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫)‪e2(t‬‬ ‫‪-1‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫]‪time [sec‬‬ ‫‪1.5‬‬ ‫‪1‬‬ ‫)‪e3(t‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.30‬שגיאות )סימולציה עם בקר )‪ ,(2.2.27‬תנאי התחלה ‪ θ ( 0 ) = 180o‬ו‬ ‫והחלפת ) ‪ θ ( t‬ב‪ θ ( t ) -‬לפי‪((2.2.36) -‬‬ ‫‪52‬‬ ‫‪Trajectory‬‬ ‫‪Trajectory‬‬ ‫‪20‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫]‪y [m‬‬ ‫‪5‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪2‬‬ ‫‪4‬‬ ‫]‪x [m‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪0‬‬ ‫]‪y [m‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪8‬‬ ‫‪-2‬‬ ‫‪6‬‬ ‫‪2‬‬ ‫‪4‬‬ ‫]‪x [m‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫‪-4‬‬ ‫גרף ‪ – 2.31‬מסלול כלי הרכב )מימין‪ ,‬סימולציה עם בקר )‪ ,(2.2.27‬משמאל‪ ,‬סימולציה‬ ‫עם בקר )‪ ,(2.2.6‬נתוני הבקר ותנאי ההתחלה זהים בשני במקרים(‬ ‫כדוגמא מייצגת ליישום השיטות המפותחות במסגרת עבודה זו בכלי רכב בעל הנעה‬ ‫דיפרנציאלית )רוב העבודה עוסק בכלי רכב בעלי ארבעה גלגלים( מוצגת כאן גרסה‬ ‫מתאימה של בקר )‪ (2.2.27‬לכלי רכב זה‪ .‬במקרה של כלי רכב בעל הנעה דיפרנציאלית‬ ‫אות הבקרה הוא &‪ ω = θ‬לכן מסלול רצוי מתאים מצריך רציפות בנגזרת השנייה בלבד‬ ‫ומודל השרשרת המתקבל הינו מסדר שני וכולל שגיאת מקום ושגיאה בכיוון כלי הרכב‪.‬‬ ‫נזכיר כי בפועל סיבוב כלי הרכב במהירות זוויתית &‪ θ‬מתאפשר ע"י הנעת הגלגל הימני‬ ‫והשמאלי במהירויות שונות‪.‬‬ ‫נרשום שוב את המודל הקינמאטי של כלי רכב בעל הנעה דיפרנציאלית הנתון ב‪(1.1) -‬‬ ‫‪x& = v cos θ‬‬ ‫‪y& = v sin θ‬‬ ‫‪θ& = ω‬‬ ‫כאשר הגדלים השונים נתונים באיור ‪2.2.2‬‬ ‫‪53‬‬ ‫‪v‬‬ ‫‪ω‬‬ ‫‪θ‬‬ ‫‪y‬‬ ‫)‪f (x‬‬ ‫‪yd‬‬ ‫‪x‬‬ ‫איור ‪ – 2.2.2‬מודל כלי רכב בעל הנעה דיפרנציאלית במערכת קואורדינאטות קרטזית‬ ‫נשתמש בטרנספורמציה הבאה‪:‬‬ ‫‪e1 = f ( x ) − y‬‬ ‫)‪(2.2.37‬‬ ‫‪e2 = f ′ ( x ) cos θ − sin θ‬‬ ‫כאשר ‪ e1‬ו‪ e 2 -‬הן שגיאות במיקום ובכיוון של כלי הרכב‪.‬‬ ‫מגזירת המשוואה )‪ (2.2.37‬והצבה של מודל כלי הרכב )‪ (1.1‬תתקבל המערכת הבאה‪:‬‬ ‫‪e&1 = e 2 v‬‬ ‫)‪(2.2.38‬‬ ‫‪e& 2 = f ′′ ( x ) cos 2 θv − ( f ′ ( x ) sin θ + cos θ ) ω‬‬ ‫נבחר אות בקרה מהצורה‪:‬‬ ‫‪f ′′ ( x ) cos 2 θv − u‬‬ ‫=‪ω‬‬ ‫) ‪( f ′ ( x ) sin θ + cos θ‬‬ ‫)‪(2.2.39‬‬ ‫כאשר כאן דרוש‪ , θr − θ < π 2 − ε , ε > 0 :‬ו‪ θr ( x ) -‬כיוון המסלול הרצוי בנקודה ‪x‬‬ ‫)ראה ‪ .(2.2.21‬תנאי זה מיועד למנוע מצב בו כלי הרכב ניצב למסלול )ואות הבקרה אינו‬ ‫מוגדר(‪.‬‬ ‫מתוך שתי המשוואות האחרונות מתקבל מודל שרשרת מסדר שני‪.‬‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = u‬‬ ‫)‪(2.2.40‬‬ ‫בחירת ‪ u‬באופן הבא‪,‬‬ ‫)‪(2.2.41‬‬ ‫‪σ = ae1 + e2‬‬ ‫∞ < ‪u =  − kh ( σ ) − ae 2  v , a , k > 0 , 0 < η1 ≤ v ≤ η2‬‬ ‫‪54‬‬ ‫שימוש במשפטים ‪ 2.1‬ו‪) 2.2-‬כאשר ) ‪ ( v ( t ) = v ( t‬וקיום התנאי ‪θr − θ < π 2 − ε , ε > 0‬‬ ‫מבטיחים יציבות אסימפטוטית של הראשית ב‪ .(2.2.40) -‬במקרה זה אות הבקרה שיש‬ ‫לספק ל‪ (1.1) -‬לצורך עקיבה אחרי המסלול ) ‪ yd = f ( x‬הוא‪:‬‬ ‫)‪(2.2.42‬‬ ‫‪)v‬‬ ‫‪θ + kh ( σ ) + ae 2‬‬ ‫‪2‬‬ ‫‪( f ′′ ( x ) cos‬‬ ‫=‪ω‬‬ ‫) ‪( f ′ ( x ) sin θ + cos θ‬‬ ‫ניתן למצוא תנאים מספיקים לקיום הדרישה ‪ θr − θ < θmax = π 2 − ε , ε > 0‬ע"י‬ ‫שימוש בחסם )‪ .(2.1.23‬נניח כי המהירות הקווית של כלי הרכב חסומה באופן הבא‪:‬‬ ‫)‪(2.2.43‬‬ ‫∞ < ‪, sup v ( t ) = vmax = η2‬‬ ‫‪t‬‬ ‫‪inf v ( t ) = vmin = η1 > 0‬‬ ‫‪t‬‬ ‫מתוך ההגדרה של ‪ e2‬ב‪ (2.2.37) -‬ושימוש בחסם )‪ (2.1.23‬מתקבל‪:‬‬ ‫)‪(2.2.44‬‬ ‫) ‪sin θr cos θ − cos θr sin θ sin ( θr − θ‬‬ ‫=‬ ‫‪cos θr‬‬ ‫‪cos θr‬‬ ‫= ‪e2 = f ′ ( x ) cos θ − sin θ‬‬ ‫) ‪sin ( θr − θ‬‬ ‫) ‪≤ 3 a1e1 ( 0 ) + e2 ( 0 ) + a1 e1 ( 0‬‬ ‫‪cos θr‬‬ ‫⇒‬ ‫מכאן‪ ,‬תנאי מספיק לקיום הדרישה ‪ θr − θ < θmax = π 2 − ε , ε > 0‬הוא‪:‬‬ ‫)‪(2.2.45‬‬ ‫) ‪3 a1e1 ( 0 ) + e2 ( 0 ) + a1 e1 ( 0 ) < sin ( θmax‬‬ ‫בכל המודלים שנדונו עד כה עבור כלי רכב בעל ארבעה גלגלים‪ ,‬מיקום כלי הרכב נקבע‬ ‫במרכז הסרן המחבר את הגלגלים האחוריים )הגלגל האחורי כאשר נעשה שימוש‬ ‫במודל ‪ .(Bicycle‬ניתן לרשום מודל שבו מיקום כלי הרכב הוא מרכז הסרן המחבר את‬ ‫הגלגלים הקדמיים )הגלגל הקדמי כאשר נעשה שימוש במודל ‪ .(Bicycle‬במקרה זה ניתן‬ ‫לעשות שימוש במודל מסוג ‪ ) unicycle‬גלגל אחד בלבד( כדי לתאר את תנועת כלי הרכב‬ ‫ומודל השגיאות המתקבל הינו פשוט יותר בהשוואה למודלים אחרים שהוצגו )למעשה‬ ‫מתקבל מודל שרשרת מסדר שני כמו במקרה של כלי רכב בעל הנעה דיפרנציאלית(‪.‬‬ ‫פיתוח מודל המבוסס על רעיון דומה מוצג גם ב‪ [De Luca 1998] -‬אך הוא אינו משמש‬ ‫שם לתכנון בקר‪ .‬לצורך פיתוח מודל כלי הרכב במקרה זה נסמן את מיקום מרכז הסרן‬ ‫המחבר את הגלגלים הקדמיים ע"י ) ‪ ( x f , yf‬כפי שנתון באיור ‪ ,2.2.3‬מכאן‪:‬‬ ‫)‪(2.2.46‬‬ ‫‪x f = x + L cos θ , yf = y + L sin θ‬‬ ‫‪55‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫‪φ‬‬ ‫‪v‬‬ ‫‪yf‬‬ ‫‪y‬‬ ‫‪θ‬‬ ‫)‪f (x‬‬ ‫‪yd‬‬ ‫‪x‬‬ ‫‪xf‬‬ ‫איור ‪ – 2.2.3‬הגדרת מקום ביחס למרכז הסרן של הגלגלים הקדמיים‬ ‫מתוך גזירה של המשוואה האחרונה והצבה של מודל כלי הרכב )‪ (1.2‬מתקבל‪:‬‬ ‫)‪(2.2.47‬‬ ‫) ‪cos ( θ + φ‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫= ‪x& f = x& − L sin θθ& = ( cos θ − sin θ tan φ ) v‬‬ ‫) ‪sin ( θ + φ‬‬ ‫= ‪y& f = y& + L cos θθ& = ( sin θ + cos θ tan φ ) v‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫כאשר גם כאן‪ ,‬כמו במודל )‪ (1.2‬דרוש ‪. φ < π 2 − ε , ε > 0‬‬ ‫נגדיר את הזווית של הגלגל הקדמי במרחב ע"י ‪ , θf # θ + φ‬מתוך גזירה של ביטוי זה‬ ‫והצבה של מודל כלי הרכב )‪ (1.2‬מתקבל‪:‬‬ ‫)‪(2.2.48‬‬ ‫‪tan φ‬‬ ‫= &‪θ& f = θ& + φ‬‬ ‫‪v+w‬‬ ‫‪L‬‬ ‫כאשר אות הבקרה ‪ w‬הוא מהירות הסיבוב של הגלגל הקדמי )מהירות סיבוב ההגה(‪.‬‬ ‫נסכם את משוואות המודל‪:‬‬ ‫‪cos θf‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫‪sin θf‬‬ ‫= ‪y& f‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫‪tan φ‬‬ ‫= ‪θ& f‬‬ ‫‪v+w‬‬ ‫‪L‬‬ ‫= ‪x& f‬‬ ‫)‪(2.2.49‬‬ ‫במודל זה משתני המצב הם ) ‪ ( x f , yf , θf‬וזווית הגלגל הקדמי ביחס למסגרת כלי הרכב‬ ‫‪ φ‬הינו אות נתון )באופן מעשי אות זה נמדד ע"י חיישן(‪.‬‬ ‫‪56‬‬ ‫נשתמש בטרנספורמציה דומה לזו שהוצגה במקרה של כלי רכב בעל הנעה‬ ‫דיפרנציאלית כאשר הקואורדינאטות ‪ y f , x f‬ו‪ θf -‬מחליפות את הקואורדינאטות ‪y, x‬‬ ‫ו‪: θ -‬‬ ‫‪e1 = f ( x f ) − y f‬‬ ‫)‪(2.2.50‬‬ ‫‪e2 = f ′ ( x f ) cos θf − sin θf‬‬ ‫ב‪ e1 (2.2.50) -‬שגיאת מקום ו‪ e2 -‬שגיאה בכיוון הגלגל הקדמי של כלי הרכב )כאשר‬ ‫זווית הגלגל הקדמי ‪ θf‬נמדדת כאן ביחס למערכת קואורדינאטות קבועה במרחב(‪.‬‬ ‫מגזירת )‪ (2.2.50‬והצבה של מודל כלי הרכב )‪ (2.2.49‬מתקבל המודל הבא‪:‬‬ ‫)‪(2.2.51‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫‪e&1 = e 2‬‬ ‫‪‬‬ ‫‪( f ′ ( x f ) sin θf + cos θf ) sin φ  v − f ′ ( x ) sin θ + cos θ w‬‬ ‫‪e& 2 =  f ′′ ( x f ) cos 2 θf −‬‬ ‫‪f‬‬ ‫‪f‬‬ ‫)‪f‬‬ ‫‪‬‬ ‫( ‪ cos φ‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫שימוש באות בקרה מהצורה‪:‬‬ ‫)‪(2.2.52‬‬ ‫‪‬‬ ‫‪( f ′ ( x f ) sin θf + cos θf ) sin φ  v − u‬‬ ‫‪ f ′′ ( x f ) cos 2 θf −‬‬ ‫‪‬‬ ‫‪ cos φ‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪w=‬‬ ‫‪′‬‬ ‫‪θ‬‬ ‫‪+‬‬ ‫‪θ‬‬ ‫‪f‬‬ ‫‪x‬‬ ‫‪sin‬‬ ‫‪cos‬‬ ‫(‬ ‫)‬ ‫‪( f‬‬ ‫‪f‬‬ ‫)‪f‬‬ ‫מוביל למודל שרשרת מסדר שני‪:‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫)‪(2.2.53‬‬ ‫‪e&1 = e 2‬‬ ‫‪e& 2 = u‬‬ ‫כאשר כאן דרוש‪ φ , θr ( x f ) − θf < π 2 − ε , ε > 0 :‬ו‪ θr ( x f ) -‬כיוון המסלול הרצוי‬ ‫בנקודה ‪) x f‬ראה ‪ .(2.2.21‬התנאי ‪ θr ( x f ) − θf < π 2 − ε‬נועד למנוע מצב בו הגלגל‬ ‫הקדמי של כלי הרכב ניצב למסלול הרצוי בנקודה ‪ . x f‬בחירת ‪ u‬באופן הבא‪,‬‬ ‫‪σ = ae1 + e2‬‬ ‫)‪(2.2.54‬‬ ‫שימוש‬ ‫‪v‬‬ ‫∞ < ‪≤ η2‬‬ ‫‪cos φ‬‬ ‫במשפטים‬ ‫‪2.1‬‬ ‫≤ ‪, a , k > 0 , 0 < η1‬‬ ‫ו‪2.2-‬‬ ‫)כאשר‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫‪v‬‬ ‫‪cos φ‬‬ ‫‪u =  − kh ( σ ) − ae 2 ‬‬ ‫= )‪( v(t‬‬ ‫וקיום‬ ‫התנאים‬ ‫‪ φ , θr ( x f ) − θf < π 2 − ε , ε > 0‬מבטיחים יציבות אסימפטוטית של הראשית ב‪-‬‬ ‫‪57‬‬ ‫)‪ .(2.2.53‬במקרה זה אות הבקרה שיש לספק ל‪ (1.2) -‬לצורך עקיבה אחרי המסלול‬ ‫) ‪ yd = f ( x‬הוא‪:‬‬ ‫)‪(2.2.55‬‬ ‫‪ f ′′ ( x f ) cos 2 θf + kh ( σ ) + ae 2 sin φ  v‬‬ ‫‪w =‬‬ ‫‪−‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪′‬‬ ‫‪f‬‬ ‫‪x‬‬ ‫‪sin‬‬ ‫‪θ‬‬ ‫‪+‬‬ ‫‪cos‬‬ ‫‪θ‬‬ ‫‪L  cos φ‬‬ ‫(‬ ‫)‬ ‫‪f‬‬ ‫‪f‬‬ ‫‪f‬‬ ‫‪‬‬ ‫‪ f ′′ ( x f ) cos 2 θf + kh ( σ ) + ae2 tan φ ‬‬ ‫‪=‬‬ ‫‪−‬‬ ‫‪v‬‬ ‫‪ ( f ′ ( x f ) sin θf + cos θf ) cos φ‬‬ ‫‪‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫הערות לגבי בקר )‪:(2.2.55‬‬ ‫‪ .1‬ניתן לקבל את אות הבקרה )‪ (2.2.55‬ישירות מתוך אות הבקרה )‪ (2.2.42‬שפותח עבור‬ ‫כלי רכב בעל הנעה דיפרנציאלית כאשר מחליפים את הגדלים ‪ x, θ‬ו‪ v -‬בגדלים ‪x f , θf‬‬ ‫‪tan φ‬‬ ‫‪v‬‬ ‫בהתאמה ומשתמשים בקשר ‪v‬‬ ‫ו‪-‬‬ ‫‪L‬‬ ‫‪cos φ‬‬ ‫‪. w = φ& = θ& f − θ& = ω −‬‬ ‫‪ .2‬במקרה זה לא ניתן לעשות שימוש בחסם )‪ (2.1.23‬כדי למצוא תנאים מספיקים‬ ‫המבטיחים את יציבות הבקר‪ .‬במודל כלי הרכב )וגם באות הבקרה( קיים מצב‬ ‫סינגולארי כאשר ‪ . φ = ± π 2‬הבקר הנדון כאן מאפשר וויסות של הזווית ‪θf = θ + φ‬‬ ‫)כלומר כיוון הגלגל הקדמי במרחב(‪ .‬משתנה המצב ) ‪ φ ( t‬של כלי הרכב אינו מבוקר‬ ‫באופן ישיר ע"י הבקר‪ ,‬לכן לא ניתן לעשות שימוש בחסם )‪ (2.1.23‬כדי למנוע מכלי‬ ‫הרכב להגיע למצב בו ‪. φ = ± π 2‬‬ ‫מגבלה זו המקשה כאן על הוכחה של יציבות מקומית אינה קיימת כאשר מדובר בכלי‬ ‫רכב בעל הנעה קדמית )ראה מודל )‪ .((1.3‬במקרה כזה יתקבל מודל שרשרת מהצורה‪:‬‬ ‫‪e&1 = e 2ν‬‬ ‫‪e& 2 = u‬‬ ‫)‪(2.2.56‬‬ ‫כאשר ) ‪ ν ( t‬המהירות הקווית של הגלגל הקדמי והשגיאות ) ‪ e1 ( t ) , e2 ( t‬מוגדרות ב‪-‬‬ ‫)‪ .(2.2.50‬אות הבקרה במקרה זה נתון ע"י‪:‬‬ ‫)‪(2.2.57‬‬ ‫‪ f ′′ ( x f ) cos 2 θf + kh ( σ ) + ae2 sin φ ‬‬ ‫‪−‬‬ ‫‪w =‬‬ ‫‪ ν‬‬ ‫‪‬‬ ‫‪′‬‬ ‫‪θ‬‬ ‫‪+‬‬ ‫‪θ‬‬ ‫‪f‬‬ ‫‪x‬‬ ‫‪sin‬‬ ‫‪cos‬‬ ‫‪L‬‬ ‫(‬ ‫)‬ ‫‪f‬‬ ‫‪f‬‬ ‫‪f‬‬ ‫‪‬‬ ‫‪‬‬ ‫וסינגולארית קיימת רק כאשר ‪. θr ( x f ) − θf < π 2 − ε , ε > 0‬‬ ‫‪58‬‬ ‫מתוך שיקולים דומים לאלו ששימושו בפיתוח )‪ (2.2.45‬גם כאן )כלי רכב בעל הנעה‬ ‫קדמית( ניתן להבטיח את קיום הדרישה ‪ , θr − θ < θmax = π 2 − ε , ε > 0‬אם מהירות‬ ‫הגלגל הקדמי‪,‬‬ ‫)‪(2.2.58‬‬ ‫∞ < ‪, sup ν ( t ) = νmax = η2‬‬ ‫‪t‬‬ ‫‪inf ν ( t ) = νmin = η1 > 0‬‬ ‫‪t‬‬ ‫ומתקיים התנאי הבא‪:‬‬ ‫)‪(2.2.59‬‬ ‫) ‪3 a1e1 ( 0 ) + e2 ( 0 ) + a1 e1 ( 0 ) < sin ( θmax‬‬ ‫לסיכום‪ ,‬ברור כי שיטה זו אשר נקראת כאן "עוקב גלגל קדמי" מתאימה בעיקר לכלי‬ ‫רכב בעל הנעה קדמית )שם ניתן להוכיח יציבות מקומית( ותחת מגבלות מסוימות‬ ‫ניתנת למימוש גם בכלי רכב בעל הנעה אחורית‪ .‬במקרה של כלי רכב בעל הנעה‬ ‫אחורית רצוי להפעיל בקר זה יחד עם מנגנון המגביל את זווית ההגה ומוצג ב‪,(2.2.35) -‬‬ ‫למרות העדר הוכחת יציבות )במקרה של כלי רכב בעל הנעה אחורית( יתרונו של הבקר‬ ‫ביחס לאחרים טמון בפשטותו‪ .‬סימולציות מראות כי ניתן להפיק ממנו תועלת רבה‪.‬‬ ‫בגרפים ‪ 2.28-2.32‬מוצגות תוצאות של סימולציה אשר בוצעה עם מודל של כלי רכב‬ ‫בעל ארבעה גלגלים )הנעה אחורית(‪ ,‬בקר היגוי בקואורדינאטות גלובליות )‪(2.2.55‬‬ ‫ומנגנון )‪ (2.2.35‬המגביל את זווית ההגה המקסימאלית ל‪ . 35o -‬תנאי ההתחלה של כלי‬ ‫הרכב בסימולציות הם ‪ , x ( 0 ) = 0, y ( 0 ) = 4 [ m ] , θ ( 0 ) = 0, φ ( 0 ) = 0‬המרחק בין ציר‬ ‫הגלגלים הקדמי והאחורי ]‪, L = 1 [m‬המסלול הרצוי ‪ , y ( x ) = sin ( x ) + 0.5x + 1‬המהירות‬ ‫הקווית של כלי הרכב ] ‪ v = 1 [ m / s‬ונתוני הבקר הם ) ‪. k = 4, a1 = 2, h ( σ ) = tanh ( σ‬‬ ‫בגרף ‪ 2.28‬מוצג המסלול שעבר הגלגל הקדמי ביחס למסלול הרצוי ואילו בגרף ‪2.29‬‬ ‫מוצג המסלול שעבר הגלגל האחורי ביחס למסלול הרצוי‪ .‬לעומת התכנסות הגלגל‬ ‫הקדמי‪ ,‬הגלגל האחורי אינו מתכנס למסלול הרצוי‪ .‬למעשה )במודל זה( ניתן לראות‬ ‫בגלגל האחורי נגרר של הגלגל הקדמי‪.‬‬ ‫‪59‬‬ Trajectory 8 trajectory - front wheel desired trajectory 7 6 y f [m] 5 4 3 2 1 1 2 3 4 5 6 x f [m] 7 8 9 10 ((2.2.55) ‫ – מסלול הגלגל הקדמי של כלי הרכב )סימולציה עם בקר‬2.28 ‫גרף‬ Trajectory trajectory - back wheel desired trajectory 8 7 6 y [m] 5 4 3 2 1 0 0 1 2 3 4 5 6 7 8 9 10 x [m] ((2.2.55) ‫ – מסלול הגלגל האחורי של כלי הרכב )סימולציה עם בקר‬2.29 ‫גרף‬ 60 Control Inputs 2 v(t) 1.5 1 0.5 0 0 2 4 6 time [sec] 8 10 12 0 2 4 6 time [sec] 8 10 12 2 1 w(t) 0 -1 -2 -3 -4 -5 ((2.2.55) ‫ – אותות בקרה )סימולציה עם בקר‬2.30 ‫גרף‬ Vehicle Angles θ(t) [deg] 50 0 -50 0 2 4 6 time [sec] 8 10 12 0 2 4 6 time [sec] 8 10 12 0 2 4 6 time [sec] 8 10 12 40 φ (t) 20 0 -20 -40 θf (t) [deg] 50 0 -50 ((2.2.55) ‫ – כיוון כלי הרכב וכיוון הגלגל הקדמי )סימולציה עם בקר‬2.31 ‫גרף‬ 61 ‫‪Tracking Errors‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫)‪e1(t‬‬ ‫‪-1‬‬ ‫‪-1.5‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫]‪time [sec‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫]‪time [sec‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫‪1.5‬‬ ‫‪1‬‬ ‫)‪e2(t‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫גרף ‪ – 2.32‬שגיאות הגלגל הקדמי ביחס למסלול הרצוי )סימולציה עם בקר )‪((2.2.55‬‬ ‫ניתן לפתח בקר ביחס למערכת קואורדינאטות קרטזית קבועה במרחב בגישה יותר‬ ‫כללית‪ .‬לפי גישה זו המסלול הרצוי נמדד בקואורדינאטה ‪ x‬שהיא אינה בהכרח זהה‬ ‫לקואורדינאטה ‪ x‬של כלי הרכב )ראה איור ‪ .(2.2.4‬הצורך בבקר כזה יובהר בפרק מספר‬ ‫‪ 4‬וקשור למקרה בו לא ניתן לייצג את המסלול הרצוי ע"י פונקציה אחת מהצורה‬ ‫) ‪ yd = f ( x‬ויש צורך במספר פונקציות המוגדרות על מספר מערכות צירים קבועות‬ ‫במרחב‪ .‬במקרה של בעיית ה‪ path following -‬ותכנון בקר ביחס למערכת קואורדינאטות‬ ‫קרטזית קבועה במרחב‪) ,‬ככל שידוע למחבר עבודה זו( הרעיון המוצג בהמשך ייחודי‬ ‫לעבודה זו‪.‬‬ ‫גם במקרה זה )כאשר המסלול נמדד בקואורדינאטה ‪ ( x‬יש לייצג את המסלול הרצוי על‬ ‫ידי פונקציה רציפה וגזירה שלוש פעמים‪.‬‬ ‫‪62‬‬ ‫‪φ‬‬ ‫‪v‬‬ ‫‪θ‬‬ ‫‪y‬‬ ‫)‪f (x‬‬ ‫‪yd‬‬ ‫‪x‬‬ ‫‪x‬‬ ‫איור ‪ – 2.2.4‬מודל כלי רכב בעל ארבעה גלגלים במערכת קואורדינאטות קרטזית‬ ‫נגדיר את שגיאת המקום באופן הבא‪:‬‬ ‫‪e1 = f ( x ) − y‬‬ ‫)‪(2.2.60‬‬ ‫כאשר התיאוריה כאן דורשת בחירה של קואורדינאטה ) ‪ x ( t‬המקיימת את התכונות‬ ‫הבאות‪:‬‬ ‫) ‪e1 ( t ) = 0 ⇒ x ( t ) = x ( t‬‬ ‫) ‪x& ( t ) = q1 ( t ) v ( t‬‬ ‫)‪(2.2.61‬‬ ‫) ‪q& 1 ( t ) = q 3 ( t ) v ( t‬‬ ‫) ‪q& 3 ( t ) = q 5 ( t ) v ( t ) + q 7 ( t ) w ( t‬‬ ‫תפקיד התכונה הראשונה ב‪ (2.2.61) -‬הוא להבטיח כי הקואורדינאטות ) ‪ x ( t‬ו‪x ( t ) -‬‬ ‫יתלכדו כאשר כלי הרכב נמצא על המסלול הרצוי‪ .‬התכונות הנוספות מאפשרות‬ ‫טרנספורמציה למודל שרשרת‪ .‬הפונקציה ) ‪ q1 ( t‬נתונה לבחירה של מתכנן מערכת‬ ‫הבקרה ומהווה חלק בהגדרה של הנקודה ‪) x‬אשר בה נמדד המסלול הרצוי(‪ .‬הפונקציה‬ ‫) ‪ q1 ( t‬צריכה לקיים מבנה מסוים אשר ממנו נגזרות הפונקציות ) ‪ q5 ( t ) , q3 ( t‬ו‪. q 7 ( t ) -‬‬ ‫נגדיר גם שגיאה בכיוון כלי הרכב ) ‪ e2 ( t‬ושגיאה בכיוון הגלגלים הקדמיים ) ‪ e3 ( t‬באופן‬ ‫הבא‪:‬‬ ‫‪e2 = f ′ ( x ) q1 − sin θ‬‬ ‫)‪(2.2.62‬‬ ‫‪tan φ‬‬ ‫‪L‬‬ ‫‪e3 = f ′′ ( x ) q12 + f ′ ( x ) q 3 − cos θ‬‬ ‫‪63‬‬ ‫‪sin θ tan φ‬‬ ‫רצוי להדגיש כי עבור ) ‪ x ( t ) = x ( t‬מתקבל‬ ‫‪L‬‬ ‫‪ q1 ( t ) = cos θ , q3 ( t ) = −‬והגדרת‬ ‫השגיאות ב‪ (4.2.60) -‬ו‪ (2.2.62) -‬זהה להגדרת השגיאות ב‪) (2.2.20) -‬השגיאות במקרה של‬ ‫בקר ‪(2.2.27‬‬ ‫מגזירה של )‪ (2.2.60‬ו‪ (2.2.62) -‬והצבה של מודל כלי הרכב )‪ (1.2‬תתקבל המערכת הבאה‪:‬‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = e3 v‬‬ ‫)‪(2.2.63‬‬ ‫‪‬‬ ‫‪ cos θ‬‬ ‫‪‬‬ ‫‪sin θ tan 2 φ ‬‬ ‫‪&e3 =  f ′′′ ( x ) q13 + 3f ′′ ( x ) q1q 3 + f ′ ( x ) q 5 +‬‬ ‫‪− f ′ ( x ) q7  w‬‬ ‫‪v − ‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪L‬‬ ‫‪ L cos φ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫נבחר אות בקרה מהצורה‪:‬‬ ‫)‪(2.2.64‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪sin θ tan 2 φ ‬‬ ‫‪L cos 2 φ‬‬ ‫‪−‬‬ ‫‪w =  f ′′′ ( x ) q13 + 3f ′′ ( x ) q1q 3 + f ′ ( x ) q 5 +‬‬ ‫‪v‬‬ ‫‪u‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪2‬‬ ‫‪L2‬‬ ‫‪‬‬ ‫‪ cos θ − L cos φf ′ ( x ) q 7‬‬ ‫‪‬‬ ‫כאשר מ‪ (2.3.64) -‬ברור כי יש להימנע ממצב בו ‪. cos θ − L cos 2 φf ′ ( x ) q 7 = 0‬‬ ‫מתוך הצבה של הבקר )‪ (2.3.64‬במודל )‪ (2.3.63‬מתקבל מודל שרשרת מסדר שלישי‪.‬‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = e3 v‬‬ ‫)‪(2.2.65‬‬ ‫‪e& 3 = u‬‬ ‫כאשר כאן ‪ . v ( t ) = v‬בחירת ‪ u‬באופן הבא‪,‬‬ ‫)‪(2.2.66‬‬ ‫‪σ = a1e1 + a 2e2 + e3‬‬ ‫‪u =  − kh ( σ ) − a1e2 − a 2e3  v , a1 , a 2 > 0‬‬ ‫שימוש במשפטים ‪ 2.1‬ו‪) 2.2 -‬כאשר ) ‪ ( v ( t ) = v ( t‬וקיום התנאים ‪ φ < π 2 − ε , ε > 0‬ו‪-‬‬ ‫‪ cos θ − L cos 2 φf ′ ( x ) q 7 ≠ 0‬מבטיחים יציבות אסימפטוטית של הראשית ב‪.(2.3.65) -‬‬ ‫במקרה זה אות הבקרה שיש לספק ל‪ (1.2) -‬לצורך עקיבה אחרי המסלול ) ‪yd = f ( x‬‬ ‫הוא‪:‬‬ ‫)‪(2.2.67‬‬ ‫‪‬‬ ‫‪sin θ tan 2 φ‬‬ ‫‪w =  f ′′′ ( x ) q13 + 3f ′′ ( x ) q1q 3 + f ′ ( x ) q 5 +‬‬ ‫‪L2‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪L cos 2 φ‬‬ ‫‪+ kh ( σ ) + a1e2 + a 2e3 ‬‬ ‫‪v‬‬ ‫‪2‬‬ ‫‪ cos θ − L cos φf ′ ( x ) q 7‬‬ ‫‪64‬‬ ‫גם כאן נוסיף כי במקרה ו‪ x ( t ) = x ( t ) -‬אז‬ ‫‪sin θ‬‬ ‫‪L cos 2 φ‬‬ ‫)‪(2.2.68‬‬ ‫‪, q7 ( t ) = −‬‬ ‫‪cos θ tan 2 φ‬‬ ‫‪L2‬‬ ‫‪q5 ( t ) = −‬‬ ‫ואות הבקרה )‪ (2.2.67‬זהה לאות הבקרה )‪(2.2.27‬‬ ‫‪c‬‬ ‫‪2‬‬ ‫לצורך הדגמת השיטה נבחר ‪ x ( t ) = x + e12‬כאשר ‪ c‬קבוע חיובי ) ‪.( c > 0‬‬ ‫מתוך ההגדרה של ) ‪ x ( t‬מתקבל‪:‬‬ ‫‪cos θ − ce1 sin θ‬‬ ‫) ‪1 − ce1f ′ ( x‬‬ ‫= ) ‪q1 ( t‬‬ ‫‪1‬‬ ‫‪ sin θ tan φ + ce1 cos θ tan φ‬‬ ‫‪‬‬ ‫‪+ ce22 + ce1f ′′ ( x ) q12 ‬‬ ‫‪q3 ( t ) =  −‬‬ ‫‪L‬‬ ‫‪‬‬ ‫) ) ‪ (1 − ce1f ′ ( x‬‬ ‫)‪(2.2.69‬‬ ‫‪ cos θ tan 2 φ‬‬ ‫‪+ 3ce2e3 + ce1f ′′′ ( x ) q13 + ce13f ′′ ( x ) q1q3‬‬ ‫‪q5 ( t ) =  −‬‬ ‫‪2‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪sin θ tan 2 φ ‬‬ ‫‪1‬‬ ‫‪+ ce1‬‬ ‫‪‬‬ ‫‪2‬‬ ‫‪L‬‬ ‫) ) ‪ (1 − ce1f ′ ( x‬‬ ‫‪ce1 cos θ + sin θ‬‬ ‫) ) ‪L cos 2 φ (1 − ce1f ′ ( x‬‬ ‫‪q7 ( t ) = −‬‬ ‫קל להראות כי המצב הסינגולארי ‪ 1 − ce1f ′ ( x ) = 0‬בדוגמא קשור באופן שבו קבענו את‬ ‫)‪: x(t‬‬ ‫)‪(2.2.70‬‬ ‫‪c‬‬ ‫‪x ( t ) = x + e12‬‬ ‫‪2‬‬ ‫&‪x& − ce1y‬‬ ‫) ‪1 − ce1f ′ ( x‬‬ ‫= &‪x& = x& + ce1e&1 = x& + ce1 ( f ′ ( x ) x& − y& ) ⇒ x‬‬ ‫כלומר ) ‪ x& ( t‬אינו מוגדר כאשר ‪.1 − ce1f ′ ( x ) = 0‬‬ ‫נראה גם כי המצב הסינגולארי השני ‪ cos θ − L cos 2 φf ′ ( x ) q 7 = 0‬שקול )כאשר‬ ‫) ‪ (1 ≠ ce1f ′ ( x‬למקרה בו הפרש הזוויות בין כיוון כלי הרכב‪ ,‬וכיוון המסלול הרצוי‬ ‫בנקודה ‪ , x‬שווה ל‪. ± π 2 -‬‬ ‫‪65‬‬ ‫)‪(2.2.71‬‬ ‫‪ae1 cos θ + sin θ cos θ + f ′ ( x ) sin θ‬‬ ‫=‬ ‫‪=0‬‬ ‫) ) ‪(1 − ce1f ′ ( x‬‬ ‫) ) ‪(1 − ce1f ′ ( x‬‬ ‫) ‪cos θ − L cos 2 φf ′ ( x ) q 7 = cos θ + f ′ ( x‬‬ ‫נסכם את התנאים הדרושים כדי להבטיח את יציבות הבקר‪:‬‬ ‫‪1 − ce1f ′ ( x ) > ε1 > 0‬‬ ‫‪cos θ + f ′ ( x ) sin θ > ε 2 > 0‬‬ ‫)‪(2.2.72‬‬ ‫‪φ < φmax = π 2 − ε3 , ε3 > 0‬‬ ‫כמו במקרה של בקר )‪ (2.2.27‬גם כאן ) ‪ v ( t ) = v ( t‬וקיימים החסמים‪:‬‬ ‫)‪(2.2.73‬‬ ‫∞ < ‪, η2 = sup v ( t ) = vmax‬‬ ‫‪t‬‬ ‫‪η1 = inf v ( t ) = vmin > 0‬‬ ‫‪t‬‬ ‫לכן ברור כי גם כאן ניתן ע"י שימוש בחסם )‪ ,(2.1.24‬תנאי ההתחלה של כלי הרכב‬ ‫ותכונות המסלול הרצוי‪ ,‬למצוא תנאים מספיקים המבטיחים יציבות מקומית של‬ ‫הבקר‪.‬‬ ‫בגרפים ‪ 2.33-2.38‬מוצגות תוצאות של סימולציה אשר בוצעה עם מודל של כלי רכב‬ ‫בעל ארבעה גלגלים ובקר )‪ (2.2.67‬כאשר ‪ . x ( t ) = x + e12‬תנאי ההתחלה של כלי הרכב‬ ‫בסימולציה הם ‪ , x ( 0 ) = 0, y ( 0 ) = 4 [ m ] , θ ( 0 ) = 0, φ ( 0 ) = 0‬המרחק בין ציר הגלגלים‬ ‫הקדמי והאחורי ]‪, L = 1 [m‬המסלול הרצוי ‪ , y ( x ) = sin ( x ) + 0.5x + 1‬המהירות הקווית‬ ‫] ‪ v = 1 [ m / s‬ונתוני הבקר הם ) ‪ . k = 2, a1 = 2, a 2 = 4, h ( σ ) = tanh ( σ‬בגרף ‪ 2.37‬ניתן‬ ‫לראות את ההתכנסות של הקואורדינאטה ) ‪ x ( t‬אל הקואורדינאטה ) ‪ , x ( t‬ובגרף ‪2.38‬‬ ‫ניתן לראות כי הביטוי ) ‪ 1 − ce1f ′ ( x‬אינו מתאפס בזמן ההתכנסות של כלי הרכב אל‬ ‫המסלול הרצוי‪.‬‬ ‫הערה‪ :‬פתרון המשוואה‬ ‫‪2‬‬ ‫) ‪ x ( t ) = x + e12 = x + ( f ( x ) − y‬מבוצע בסימולציה )וניתן‬ ‫לביצוע בזמן אמת( ע"י שימוש בקירוב נומרי‪.‬‬ ‫‪66‬‬ Trajectory trajectory desired trajectory 7 6 5 y [m] 4 3 2 1 0 0 1 2 3 4 5 x [m] 6 7 8 9 10 ((2.2.67) ‫ – מסלול כלי הרכב )סימולציה עם בקר‬2.33 ‫גרף‬ Control Inputs 2 v(t) 1.5 1 0.5 0 0 2 4 6 8 10 12 14 8 10 12 14 time [sec] 2 1 w(t) 0 -1 -2 -3 -4 0 2 4 6 time [sec] ((2.2.67) ‫ – אותות בקרה )סימולציה עם בקר‬2.34 ‫גרף‬ 67 ‫‪Vehicle Angles‬‬ ‫‪60‬‬ ‫‪40‬‬ ‫‪0‬‬ ‫]‪θ(t) [deg‬‬ ‫‪20‬‬ ‫‪-20‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-40‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫]‪φ (t) [deg‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.35‬כיוון כלי הרכב וכיוון הגלגלים הקדמיים )סימולציה עם בקר )‪((2.2.67‬‬ ‫‪Tracking Errors‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫)‪e1(t‬‬ ‫‪-1‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪-1.5‬‬ ‫‪0.4‬‬ ‫‪0.3‬‬ ‫)‪e2(t‬‬ ‫‪0.2‬‬ ‫‪0.1‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪0.6‬‬ ‫‪0.4‬‬ ‫)‪e3(t‬‬ ‫‪0.2‬‬ ‫‪0‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-0.2‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.36‬שגיאות כלי הרכב ביחס למסלול הרצוי )סימולציה עם בקר )‪((2.2.67‬‬ ‫‪68‬‬ ‫)‪(x) Vs. (x+dx‬‬ ‫‪12‬‬ ‫)‪(x+dx‬‬ ‫)‪(x‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪6‬‬ ‫‪8‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ x ( t ) – 2.37‬לעומת ) ‪) x ( t‬סימולציה עם בקר )‪((2.2.67‬‬ ‫) ) ‪ x ( t‬מסומן בגרף ע"י ) ‪( ( x + dx‬‬ ‫) '‪( 1-a*e1*f‬‬ ‫‪2.6‬‬ ‫‪2.4‬‬ ‫‪2.2‬‬ ‫‪2‬‬ ‫‪1.8‬‬ ‫‪1.6‬‬ ‫‪1.4‬‬ ‫‪1.2‬‬ ‫‪1‬‬ ‫‪0.8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪6‬‬ ‫‪8‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪0.6‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ – 2.38‬הביטוי ) ‪) 1 − ce1f ′ ( x‬סימולציה עם בקר )‪((2.2.67‬‬ ‫‪69‬‬ ‫]‪(x) & (x+dx) [m‬‬ ‫‪6‬‬ ‫‪ - 2.3‬תכנון בקר לכלי רכב אוטונומי בגישה של מערכת קואורדינאטות פולארית קבועה‬ ‫במרחב‬ ‫בסעיף זה מוצג בקר המשמש לנהיגה אוטונומית של כלי רכב בעל ארבעה גלגלים‬ ‫כאשר מצב כלי הרכב והמסלול הרצוי נתונים במערכת קואורדינאטות פולארית קבועה‬ ‫במרחב‪ .‬מלבד העניין הקשור בפיתוח בקר כזה‪ ,‬חשיבותו המעשית תובהר בפרק ‪4‬‬ ‫)נושא זה קשור בעקיבה אחרי מסלול רצוי המורכב ממספר פונקציות המוגדרות על‬ ‫מערכות צירים שונות(‪ .‬נוסיף עוד בעניין זה‪ ,‬כי מערכת קואורדינאטות פולארית‬ ‫מאפשרת ייצוג מסלולים אשר אינם ניתנים לייצוג ע"י פונקציה יחידה במערכת‬ ‫קואורדינאטות קרטזית )ראה סימולציה בהמשך(‪ .‬פיתוח הבקר מתבסס על מודל‬ ‫קינמטי של כלי רכב בעל ארבעה גלגלים במערכת קואורדינאטות פולאריות כפי שנתון‬ ‫באיור ‪ 2.3.1‬ובמשוואה )‪.(2.3.1‬‬ ‫‪φ‬‬ ‫‪v‬‬ ‫‪θ‬‬ ‫‪y‬‬ ‫‪ϕ‬‬ ‫‪r‬‬ ‫‪α‬‬ ‫‪x‬‬ ‫איור ‪ – 2.3.1‬מודל כלי רכב בעל ארבעה גלגלים במערכת קואורדינאטות פולארית‬ ‫‪v‬‬ ‫‪cos ϕ‬‬ ‫‪r‬‬ ‫‪r& = v sin ϕ‬‬ ‫= &‪α‬‬ ‫)‪(2.3.1‬‬ ‫‪ tan φ cos ϕ ‬‬ ‫‪ϕ& = θ& + α& = ‬‬ ‫‪+‬‬ ‫‪v‬‬ ‫‪r ‬‬ ‫‪ L‬‬ ‫‪φ& = w‬‬ ‫במודל זה משתני המצב‪ r :‬רדיוס ו‪ α -‬זווית )ביחס לציר ‪ y‬של מערכת קרטזית(‬ ‫משמשים לייצוג המקום של כלי הרכב במרחב‪ ,‬וכיוונו נקבע לפי הזווית ‪ ϕ‬אשר נמדדת‬ ‫‪70‬‬ ‫ביחס לקו ניצב לרדיוס‪ .‬משתני המצב ‪ r , α‬ו‪ ϕ -‬במערכת הפולארית אנלוגיים‬ ‫)בהתאמה( למשתני המצב ‪ y ,x‬ו‪ θ -‬במערכת הקרטזית‪ .‬לדוגמא כאשר כלי הרכב נוסע‬ ‫לאורך מסלול בו ‪ r‬קבוע )מעגל( אז הזווית ‪) ϕ‬כיוון כלי הרכב( שווה אפס‪ ,‬מסלול כזה‬ ‫אנלוגי למסלול בו ‪ y‬קבוע וזווית ‪ θ‬שווה אפס‪ ,‬במערכת קרטזית‪ .‬מלבד המצבים‬ ‫הסינגולאריים ‪ φ = ± π 2‬אשר קיימים גם במודל כלי רכב ביחס למערכת קואורדינאטות‬ ‫קרטזית‪ ,‬למודל הפולארי מצב סינגולארי נוסף כאשר ‪) r = 0‬במצב זה הזווית ‪ α‬ו‪ϕ -‬‬ ‫אינן מוגדרות(‪.‬‬ ‫במקרה של בקר המבוסס על שימוש במערכת קואורדינאטות פולארית‪ ,‬מסלול הייחוס‬ ‫נתון ע"י פונקציה מהצורה ) ‪ , rd = f ( α‬רציפה וגזירה לפחות שלוש פעמים‪ .‬מצב כלי‬ ‫הרכב ביחס למסלול הרצוי נקבע ע"י הגדרת שגיאות באופן הבא‪:‬‬ ‫‪e1 = f ( α ) − r‬‬ ‫‪f ′ ( α ) cos ϕ‬‬ ‫‪− sin ϕ‬‬ ‫‪r‬‬ ‫‪f ′′ ( α ) cos 2 ϕ‬‬ ‫‪f ′ ( α ) sin ϕ cos ϕ f ′ ( α ) sin ϕ tan φ tan φ cos ϕ cos 2 ϕ‬‬ ‫‪−2‬‬ ‫‪−‬‬ ‫‪−‬‬ ‫‪−‬‬ ‫= ‪e3‬‬ ‫‪Lr‬‬ ‫‪L‬‬ ‫‪r‬‬ ‫‪r2‬‬ ‫‪r2‬‬ ‫= ‪e2‬‬ ‫)‪(2.3.2‬‬ ‫כאשר כאן דרוש ‪. r > ε1 , φ < π 2 − ε2 , ε1 , ε 2 > 0‬‬ ‫כמו במודלים שהוצגו קודם לכן‪ ,‬גם כאן ‪ e1‬שגיאת מקום‪ e 2 ,‬שגיאת כיוון ו‪ e 3 -‬שגיאה‬ ‫בכיוון הגלגלים הקדמיים של כלי הרכב‪.‬‬ ‫הטרנספורמציה הנתונה ב‪ (2.3.2) -‬ומבוססת על מודל פולארי ייחודית לעבודה זו‬ ‫)שימוש במודל פולארי לצורך בקרה של כלי רכב לא בהקשר של מודל שרשרת קיים גם‬ ‫בעבודות אחרות לדוגמא‪ [Chwa 2004] ,[Yang et al. 1998] :‬ו‪([Indiveri 1999] -‬‬ ‫מגזירת )‪ (2.3.2‬והצבה של מודל כלי הרכב )‪ (2.3.1‬תתקבל המערכת הבאה‪:‬‬ ‫‪)v‬‬ ‫)‪(2.3.3‬‬ ‫‪v‬‬ ‫‪φ‬‬ ‫‪2‬‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = e3 v‬‬ ‫(‬ ‫‪f ′′′ ( α ) cos3 ϕ − 6f ′′ ( α ) sin ϕ cos 2 ϕ + 2f ′ ( α ) cos ϕ 3 sin 2 ϕ − cos2 ϕ‬‬ ‫‪) v − f ′ ( α ) cos ϕ tan‬‬ ‫(‬ ‫‪r3‬‬ ‫‪−f ′′ ( α ) sin ϕ cos ϕ tan φ + f ′ ( α ) tan φ sin 2 ϕ − cos 2 ϕ‬‬ ‫= ‪e& 3‬‬ ‫‪+3‬‬ ‫‪Lr 2‬‬ ‫‪L2 r‬‬ ‫‪f ′ ( α ) sin ϕ + r cos ϕ‬‬ ‫‪sin ϕ cos 2 ϕ‬‬ ‫‪tan 2 φ sin ϕ‬‬ ‫‪sin ϕ cos ϕ tan φ‬‬ ‫‪v‬‬ ‫‪v+3‬‬ ‫‪v−‬‬ ‫‪w‬‬ ‫‪+3‬‬ ‫‪+‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪Lr‬‬ ‫‪r‬‬ ‫‪L‬‬ ‫‪Lr cos 2 φ‬‬ ‫‪71‬‬ ‫ע"י בחירה של אות בקרה מהצורה‪:‬‬ ‫)‬ ‫)‪(2.3.4‬‬ ‫‪φ‬‬ ‫‪2‬‬ ‫(‬ ‫‪ f ′′′ ( α ) cos3 ϕ − 6f ′′ ( α ) sin ϕ cos 2 ϕ + 2f ′ ( α ) cos ϕ 3 sin 2 ϕ − cos2 ϕ‬‬ ‫‪w = ‬‬ ‫‪‬‬ ‫‪r3‬‬ ‫‪‬‬ ‫‪) − f ′ ( α ) cos ϕ tan‬‬ ‫(‬ ‫‪−f ′′ ( α ) sin ϕ cos ϕ tan φ + f ′ ( α ) tan φ sin 2 ϕ − cos 2 ϕ‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪Lr‬‬ ‫‪Lr‬‬ ‫‪+3‬‬ ‫‪‬‬ ‫‪sin ϕ cos 2 ϕ tan 2 φ sin ϕ‬‬ ‫‪sin ϕ cos ϕ tan φ ‬‬ ‫‪Lr cos2 φ‬‬ ‫‪+‬‬ ‫‪+‬‬ ‫‪−‬‬ ‫‪3‬‬ ‫‪v‬‬ ‫‪u‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪Lr‬‬ ‫‪r2‬‬ ‫‪L2‬‬ ‫‪ f ′ ( α ) sin ϕ + r cos ϕ‬‬ ‫‪‬‬ ‫‪+3‬‬ ‫כאשר ‪ , f ′ ( α ) sin ϕ + r cos ϕ ≠ 0‬מתקבל מודל בצורת שרשרת מסדר שלישי‪.‬‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = e3 v‬‬ ‫)‪(2.3.5‬‬ ‫‪e& 3 = u‬‬ ‫נגדיר את הזווית ‪ ϕr‬באופן הבא‪:‬‬ ‫) ‪df ( α‬‬ ‫‪sin ϕr‬‬ ‫‪=r‬‬ ‫‪dα‬‬ ‫‪cos ϕr‬‬ ‫)‪(2.3.6‬‬ ‫= )‪f ′(α‬‬ ‫הזווית ‪ ϕr‬היא הזווית בין כיוון המסלול הרצוי ובין קו ייחוס ניצב לרדיוס )הגדרה זו‬ ‫אנלוגית להגדרת הזווית ‪ θr‬במשוואה )‪ (2.2.21‬כאשר נדון בקר ‪.(2.2.27‬‬ ‫מכאן‪:‬‬ ‫)‪(2.3.7‬‬ ‫) ‪cos ( ϕr − ϕ‬‬ ‫‪sin ϕr‬‬ ‫‪sin ϕ + r cos ϕ = r‬‬ ‫‪cos ϕr‬‬ ‫‪cos ϕr‬‬ ‫‪f ′ ( α ) sin ϕ + r cos ϕ = r‬‬ ‫כלומר המצב הסינגולארי הנובע משימוש באות בקרה )‪ (2.3.4‬הוא תוצאה של זווית‬ ‫בגודל ‪ ± π 2‬בין כלי הרכב ובין המסלול הרצוי‪ ,‬מצב זה אנלוגי למצב הסינגולארי בו‬ ‫כלי הרכב ניצב למסלול במקרה של בקר )‪.(2.2.27‬‬ ‫בחירת ‪ u‬באופן הבא‪,‬‬ ‫‪σ = a1e1 + a 2e2 + e3‬‬ ‫)‪(2.3.8‬‬ ‫שימוש‬ ‫‪u =  − kh ( σ ) − a1e2 − a 2e3  v , a1 , a 2 > 0‬‬ ‫במשפטים‬ ‫‪2.1‬‬ ‫ו‪-‬‬ ‫‪2.2‬‬ ‫)כאשר‬ ‫)‪( v (t) = v(t‬‬ ‫וקיום‬ ‫התנאים‬ ‫‪ r > ε, ϕr − ϕ , φ < π 2 − ε , ε > 0‬מבטיחים יציבות אסימפטוטית של הראשית ב‪-‬‬ ‫‪72‬‬ ‫)‪ .(2.3.5‬במקרה זה אות הבקרה שיש לספק ל‪ (1.2) -‬לצורך עקיבה אחרי המסלול‬ ‫) ‪ rd = f ( α‬הוא‪:‬‬ ‫)‬ ‫‪φ‬‬ ‫)‪(2.3.9‬‬ ‫‪2‬‬ ‫(‬ ‫‪ f ′′′ ( α ) cos3 ϕ − 6f ′′ ( α ) sin ϕ cos2 ϕ + 2f ′ ( α ) cos ϕ 3 sin 2 ϕ − cos 2 ϕ‬‬ ‫‪w =‬‬ ‫‪‬‬ ‫‪r3‬‬ ‫‪‬‬ ‫‪) − f ′ ( α ) cos ϕ tan‬‬ ‫(‬ ‫‪−f ′′ ( α ) sin ϕ cos ϕ tan φ + f ′ ( α ) tan φ sin 2 ϕ − cos 2 ϕ‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪Lr‬‬ ‫‪Lr‬‬ ‫‪‬‬ ‫‪sin ϕ cos 2 ϕ tan 2 φ sin ϕ‬‬ ‫‪sin ϕ cos ϕ tan φ‬‬ ‫‪Lr cos 2 φ v‬‬ ‫‪+‬‬ ‫‪+‬‬ ‫‪3‬‬ ‫‪+‬‬ ‫‪kh‬‬ ‫‪σ‬‬ ‫‪+‬‬ ‫‪a‬‬ ‫‪e‬‬ ‫‪+‬‬ ‫‪a‬‬ ‫‪e‬‬ ‫(‬ ‫)‬ ‫‪1 2‬‬ ‫‪2 3‬‬ ‫‪Lr‬‬ ‫‪r2‬‬ ‫‪L2‬‬ ‫‪ f ′ ( α ) sin ϕ + r cos ϕ‬‬ ‫כמו במקרה של בקר )‪ (2.2.27‬גם כאן )ע"פ עקרונות דומים( נמצא תנאים מספיקים‬ ‫הקשורים במצב ההתחלתי של כלי הרכב ובתכונות המסלול הרצוי‪ ,‬כך שיתקיימו אי‬ ‫השוויוניים הבאים‪ ,‬המאפשרים יציבות מקומית של החוג הסגור‪:‬‬ ‫‪r > rmin = ε1 , ε1 > 0‬‬ ‫‪ϕr − ϕ < ϕmax = π 2 − ε 2 , ε 2 > 0‬‬ ‫)‪(2.3.10‬‬ ‫‪φ < φmax = π 2 − ε3 , ε3 > 0‬‬ ‫נניח כי המסלול שאחריו רוצים לעקוב‪ ,‬המהירות הקווית של כלי הרכב וקבועי הבקר‬ ‫‪ a1‬ו‪ a 2 -‬מקיימים‪:‬‬ ‫‪′ < ∞ , sup f ′′ ( α ) = fmax‬‬ ‫∞ < ‪′′‬‬ ‫‪inf f ( α ) = fmin > 0 , sup f ′ ( α ) = fmax‬‬ ‫‪α‬‬ ‫)‪(2.3.11‬‬ ‫‪α‬‬ ‫‪α‬‬ ‫‪inf v ( t ) = vmin > 0‬‬ ‫∞ < ‪, sup v ( t ) = vmax‬‬ ‫‪t‬‬ ‫‪t‬‬ ‫‪ 0‬‬ ‫‪1  ‬‬ ‫‪sup exp  ‬‬ ‫‪ t  = c3 , c 4 = c3 + 1‬‬ ‫‪t‬‬ ‫‪  − a1 − a 2  ‬‬ ‫במודל השרשרת )‪ v ( t ) = v ( t ) (2.3.5‬לכן‬ ‫)‪(2.3.12‬‬ ‫‪, η2 = vmax‬‬ ‫‪η1 = vmin‬‬ ‫מתוך ההגדרה של ‪ e1‬ב‪ ,(2.3.2) -‬שימוש בחסם )‪ (2.1.24‬והצבה של )‪:(2.3.11‬‬ ‫‪r = f ( α ) − e1 ≥ f ( α ) − e1‬‬ ‫)‪(2.3.13‬‬ ‫) ‪a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0‬‬ ‫) ‪e1 ( 0‬‬ ‫‪− c4‬‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪73‬‬ ‫‪r > fmin − c3‬‬ ‫‪+3‬‬ ‫‪+3‬‬ :‫( נתון ע"י‬2.3.10 ‫ )ראה‬r > rmin ‫( תנאי מספיק לקיום אי השוויון‬2.3.13) ‫מתוך‬ fmin − c3 c3 e1 ( 0 ) e2 ( 0 ) e1 ( 0 ) e2 ( 0 ) + c4 − c4 a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0 ) a1 a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0 ) a1 > rmin (2.3.14) < fmin − rmin :(2.3.11) ‫( והצבה של‬2.1.24) ‫ שימוש בחסם‬,(2.3.2) -‫ ב‬e2 ‫מתוך ההגדרה של‬ e2 = f ′ ( α ) cos ϕ sin ϕr cos ϕ − cos ϕr sin ϕ sin ( ϕr − ϕ ) − sin ϕ = = r cos ϕr cos ϕr ⇒ a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0 ) e ( 0) sin ( ϕr − ϕ ) < c3 1 + c4 e2 ( 0 ) cos ϕr a1 ⇒ sin ( ϕr − ϕ ) < c3 (2.3.15) a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0 ) e1 ( 0 ) + c4 e2 ( 0 ) a1 :‫( דרוש‬2.3.10) -‫כדי לקיים את הדרישה השנייה ב‬  e1 ( 0 ) a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0 ) + c4  c3  e2 ( 0 ) a1    < sin ( ϕmax )   (2.3.16) :(2.3.11) ‫( והצבה של‬2.1.24) ‫ שימוש בחסם‬,(2.3.2) -‫ ב‬e3 ‫מתוך ההגדרה של‬ e3 = = f ′′ ( α ) cos 2 ϕ r2 f ′′ ( α ) cos 2 ϕ r2 −2 −2 f ′ ( α ) sin ϕ cos ϕ r2 f ′ ( α ) sin ϕ cos ϕ r2 −  cos 2 ϕ tan φ  f ′ ( α ) sin ϕ cos + ϕ  − L  r r  − tan φ cos ( ϕr − ϕ ) cos 2 ϕ − cos ϕr L r ⇒ 2 f ′ ( α ) sin ϕ cos ϕ cos 2 ϕ tan φ cos ( ϕr − ϕ ) f ′′ ( α ) cos ϕ = − − − e3 2 cos ϕr L r r2 r2 ′′ f′ tan φ cos ( ϕr − ϕ ) fmax 1 ≤ 2 + 2 ma2 x + cos ϕr r L r r  c a2 + a2  e ( 0) 2 + 4 1 + 1 a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1 e2 ( 0 ) a1     74 (2.3.17) ‫נניח כי )‪ (2.3.14‬ו‪ (2.3.16) -‬מתקיימים‪ ,‬מכאן תנאי מספיק לקיום אי השוויון‬ ‫) ‪) tan φ < tan ( φmax‬ראה ‪ (2.3.10‬נתון ע"י‪:‬‬ ‫)‪(2.3.18‬‬ ‫‪ c a2 + a2‬‬ ‫‪‬‬ ‫‪′′‬‬ ‫‪′‬‬ ‫‪fmax‬‬ ‫‪fmax‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪ 4 1‬‬ ‫) ‪ a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪+‬‬ ‫‪+‬‬ ‫‪+‬‬ ‫‪+‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪rmin ‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫‪rmin‬‬ ‫‪rmin‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪tan φmax cos ϕmax‬‬ ‫‪L‬‬ ‫≤‬ ‫) ‪e1 ( 0‬‬ ‫) ‪e2 ( 0‬‬ ‫‪+c3 a12 + a 22‬‬ ‫)קיום הדרישה השלישית ב‪ (2.3.10) -‬מחייב גם קיום הדרישות הראשונה והשנייה(‪.‬‬ ‫הערה‪ :‬ניתן ע"י שימוש בטרנספורמציות מעט שונות מזו המוצגת ב‪) (2.3.2) -‬למעשה‬ ‫הגדרות שונות של שגיאות כלי הרכב ביחס למסלול הרצוי( לקבל מודלים בצורת‬ ‫שרשרת שבהם &‪ v ( t ) = rα‬או &‪ . v ( t ) = α‬במקרים אלא הפונקציה ) ‪ v ( t‬תלויה במצב כלי‬ ‫הרכב‪ ,‬עובדה זו עשויה )עבור מצבים מסוימים של כלי הרכב‪ ,‬ראה הערה ‪ 4‬בקר ‪(2.2.27‬‬ ‫להביא לקצב התכנסות נמוך מאוד )המקרה בו &‪ v ( t ) = rα‬אנלוגי למקרה בו &‪v ( t ) = x‬‬ ‫במערכת קרטזית‪ ,‬ועבור &‪ v ( t ) = α‬יתקבל קצב התכנסות איטי מאוד עבור רדיוסים‬ ‫גדולים(‪.‬‬ ‫בגרפים ‪ 2.39-2.42‬מוצגות תוצאות של סימולציה אשר בוצעה עם מודל של כלי רכב‬ ‫בעל ארבעה גלגלים ובקר היגוי בקואורדינאטות פולאריות )‪ .(2.3.9‬תנאי ההתחלה של‬ ‫כלי הרכב בסימולציות הם ‪ , x ( 0 ) = −1 [ m ] , y ( 0 ) = 0, θ ( 0 ) = 180 [deg ] , φ ( 0 ) = 0‬המרחק‬ ‫בין ציר הגלגלים הקדמי והאחורי ]‪, L = 1 [m‬המסלול הרצוי ‪, r ( α ) = sin ( 4α ) + 5‬‬ ‫המהירות‬ ‫הקווית‬ ‫של‬ ‫כלי‬ ‫הרכב‬ ‫]‪v = 1 [ m / s‬‬ ‫ונתוני‬ ‫הבקר‬ ‫הם‬ ‫) ‪. k = 2, a1 = 2, a 2 = 4, h ( σ ) = tanh ( σ‬‬ ‫תנאי ההתחלה הנתונים כאן ביחס למערכת קואורדינאטות קרטזית אנלוגיים לתנאי‬ ‫ההתחלה ] ‪ r ( 0 ) = 1 [ m ] , α ( 0 ) = −90 [deg ] , ϕ ( 0 ) = 90 [deg‬ביחס למערכת קואורדינאטות‬ ‫פולארית‪.‬‬ ‫‪75‬‬ Trajectory trajectory desired trajectory 7 6 5 y [m] 4 3 2 1 0 -1 -6 -4 -2 0 x [m] 2 4 ((2.3.9) ‫– מסלול כלי הרכב )סימולציה עם בקר פולארי‬2.39 ‫גרף‬ Control Inputs 2 v(t) 1.5 1 0.5 0 0 5 10 15 20 25 15 20 25 time [sec] 1 w(t) 0.5 0 -0.5 -1 0 5 10 time [sec] ((2.3.9) ‫– אותות בקרה )סימולציה עם בקר פולארי‬2.40 ‫גרף‬ 76 ‫‪Polar data‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫)‪r(t‬‬ ‫‪2‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪100‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫]‪α (t) [deg‬‬ ‫‪50‬‬ ‫‪-100‬‬ ‫]‪time [sec‬‬ ‫‪100‬‬ ‫]‪phi (t) [deg‬‬ ‫‪50‬‬ ‫‪0‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-50‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ –2.41‬משתני מצב פולאריים )סימולציה עם בקר פולארי )‪((2.3.9‬‬ ‫)משתנה המצב ) ‪ ϕ ( t‬מסומן בגרף ע"י ) ‪( phi ( t‬‬ ‫‪Tracking Errors‬‬ ‫‪4‬‬ ‫‪3‬‬ ‫)‪e1(t‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪0‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫)‪e2(t‬‬ ‫‪-0.5‬‬ ‫‪-1‬‬ ‫]‪time [sec‬‬ ‫‪0.4‬‬ ‫‪0.2‬‬ ‫)‪e3(t‬‬ ‫‪0‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-0.2‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ –2.42‬שגיאות ביחס למסלול הרצוי )סימולציה עם בקר פולארי )‪((2.3.9‬‬ ‫‪77‬‬ ‫‪ – 2.4‬תכנון בקרים לכלי רכב אוטונומיים בגישה של מערכת קואורדינאטות מקומית‬ ‫הבקר המוצג כאן משמש לצורך נהיגה אוטונומית של כלי רכב בעל ארבעה גלגלים‬ ‫לאורך מסלול הניתן לתיאור ע"י עקום פרמטרי מהצורה ) ‪) x d = f x ( s ) , yd = f y ( s‬ראה‬ ‫איור )‪ .((2.4.1‬לצורך פיתוח הבקר מוגדרות שתי מערכות צירים‪ (X, Y ) :‬מערכת‬ ‫קואורדינאטות קבועה במרחב )אינרציאלית( ו‪ (x, y ) -‬מערכת )מקומית( רתומה לכלי‬ ‫הרכב )כך שראשיתה במרכז הסרן המחבר את הגלגלים המונעים וציר ‪ x‬בכיוון‬ ‫הנסיעה(‪ .‬המודל הקינמטי של כלי רכב בעל ארבעה גלגלים ביחס למערכת‬ ‫קואורדינאטות קבועה במרחב נתון במשוואה )‪ ,(1.2‬פיתוח הבקר בסעיף זה מבוסס על‬ ‫מודל קינמטי של המסלול הרצוי במערכת המקומית הרתומה לכלי הרכב )המסלול‬ ‫הרצוי נע יחסית למערכת זו כתוצאה מתנועת כלי הרכב במרחב(‪ .‬פעולת הבקר‬ ‫מבוססת על מדידה של המסלול במערכת המקומית ולא מצריכה מדידה של מצב כלי‬ ‫הרכב במרחב‪ .‬פיתוח מודל קינמאטי זה נשען על כלים המוצגים ב‪[Frezza at al.1998] -‬‬ ‫ו‪ .[Ma at al. 1999] -‬במאמרים אלה מוצג מודל דומה לכלי רכב בעל הנעה דיפרנציאלית‬ ‫)לא בגישה של מודל שרשרת(‪ ,‬הטרנספורמציה למודל שרשרת המוצגת בהמשך עבור‬ ‫כלי רכב בעל ארבעה גלגלים ייחודית לעבודה זו‪.‬‬ ‫תיאור סכמאתי של מערכות הצירים מופיע באיור ‪ 2.4.1‬כאשר הנקודה ) ‪ (x c , y c‬והזווית‬ ‫‪ θ‬מתארים את מצב כלי הרכב במרחב‪ P ,‬ייצוג נקודת מסלול במערכת ) ‪ (X, Y‬ו‪p -‬‬ ‫ייצוג נקודת מסלול במערכת ) ‪. (x, y‬‬ ‫‪Y‬‬ ‫) ‪p = ( x, y‬‬ ‫) ‪fx (s ) , fy (s‬‬ ‫‪x‬‬ ‫) ‪P = ( X, Y‬‬ ‫‪y‬‬ ‫‪θ‬‬ ‫‪X‬‬ ‫‪yc‬‬ ‫‪xc‬‬ ‫איור ‪ – 2.4.1‬הגדרת מערכות צירים‬ ‫‪78‬‬ ‫כדי לפתח את המשוואות הקינמטיות של נקודת מסלול הנתונה במערכת‬ ‫קואורדינאטות רתומה לכלי הרכב נרשום העתקה של נקודה מהמערכת הקבועה ) ‪(X, Y‬‬ ‫אל המערכת המקומית ) ‪. ( x , y‬‬ ‫‪p = RP‬‬ ‫)‪(2.4.1‬‬ ‫כאשר המטריצה ‪ R‬הינה מטריצת סיבוב )רוטציה( ומוגדרת ע"י‪:‬‬ ‫‪ cos θ sin θ − x c cos θ − y c sin θ ‬‬ ‫‪R =  − sin θ cos θ x c sin θ − y c cos θ ‬‬ ‫‪ 0‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪1‬‬ ‫מתוך גזירה של )‪ (2.4.1‬מתקבל‪:‬‬ ‫‪& = RR‬‬ ‫‪& −1p‬‬ ‫‪p& = RP‬‬ ‫)‪(2.4.2‬‬ ‫כאשר‬ ‫‪ − sin θθ& cos θθ& − x& c cos θ + x c sin θ θ& − y& c sin θ − y c cos θθ& ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪R& =  − cos θθ& − sin θθ& x& c sin θ + x c cos θ θ& − y& c cos θ + y c sin θθ& ‬‬ ‫‪ 0‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪1‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪cos θ − sin θ x c ‬‬ ‫‪=  sin θ cos θ y c ‬‬ ‫‪ 0‬‬ ‫‪0‬‬ ‫‪1 ‬‬ ‫‪−1‬‬ ‫‪R‬‬ ‫נסמן את ‪ x‬ו‪ y -‬במודל כלי הרכב )משוואה ‪ (1.2‬ע"י ‪ x c‬ו‪ y c -‬בהתאמה ונציב את ‪, x& c‬‬ ‫‪ y& c‬ו‪ θ& -‬מתוך מודל זה ב‪. R& -‬‬ ‫)‪(2.4.3‬‬ ‫‪ 0 ω −v‬‬ ‫‪−1‬‬ ‫&‬ ‫‪RR =  −ω 0 0 ‬‬ ‫‪ 0 0 1 ‬‬ ‫‪( x c sin θ − yc cos θ ) ω − v ‬‬ ‫⇒ ‪( x c cos θ + yc sin θ ) ω ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪1‬‬ ‫‪ − sin θω cos θω‬‬ ‫‪‬‬ ‫&‬ ‫‪R =  − cos θω − sin θω‬‬ ‫‪ 0‬‬ ‫‪0‬‬ ‫‪‬‬ ‫מתוך הצבה של )‪ (2.4.3‬ב‪ (2.4.2) -‬מתקבל המודל הקינמטי‪:‬‬ ‫)‪(2.4.4‬‬ ‫‪x& = θ& y − v‬‬ ‫‪y& = −θ& x‬‬ ‫⇒‬ ‫‪ 0 θ& − v   x ‬‬ ‫‪ x&   0 θ& − v ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪p& =  y&  =  −θ& 0 0  p =  −θ& 0 0   y ‬‬ ‫‪ 0 0 1   1 ‬‬ ‫‪ 1   0 0 1 ‬‬ ‫‪‬‬ ‫‪‬‬ ‫כאשר ‪ v‬ו‪ θ& -‬הן המהירות הקווית והמהירות הזוויתית של כלי הרכב ו‪x,y -‬‬ ‫קואורדינאטות של נקודת מסלול ביחס למערכת צירים רתומה לכלי הרכב‪ .‬התוצאה‬ ‫‪79‬‬ ‫האחרונה נכונה גם עבור כלי רכב בעל הנעה דיפרנציאלית )שם ‪ θ& = ω‬ראה משוואה‬ ‫)‪ ((1.1‬וגם עבור כלי רכב בעל ארבעה גלגלים‪ .‬במקרה של כלי רכב בעל ארבעה גלגלים‪:‬‬ ‫‪ tan φ‬‬ ‫‪‬‬ ‫‪x& = θ& y − v = ‬‬ ‫‪y − 1 v‬‬ ‫‪ L‬‬ ‫‪‬‬ ‫‪tan φ‬‬ ‫‪y& = −θ& x = −‬‬ ‫‪xv‬‬ ‫‪L‬‬ ‫)‪(2.4.5‬‬ ‫הערה‪ :‬עבור ‪ x = 0‬מתקבל ‪ , y& = 0‬תוצאה זו נובעת מתכונת ה "גלגול ללא החלקה" של‬ ‫מודל כלי הרכב‪ .‬נדגיש בהקשר זה כי ראשית מערכת הצירים נמצאת במרכז הסרן‬ ‫המחבר את הגלגלים המונעים )אחוריים במקרה של כלי רכב בעל ארבעה גלגלים(‪.‬‬ ‫לצורך המשך פיתוח מערכת הבקרה דרושה הנחה כי באופן מקומי המסלול הנמדד‬ ‫במערכת הצירים של כלי הרכב ניתן לביטוי ע"י פונקציה חד‪-‬ערכית ) ‪ y = f (x , t‬כמוראה‬ ‫באיור הבא‪:‬‬ ‫‪y‬‬ ‫) ‪y = f (x , t‬‬ ‫‪x‬‬ ‫איור ‪ - 2.4.2‬המסלול הרצוי במערכת הרכב‬ ‫מתוך כלל השרשרת והצבה של )‪(2.4.4‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫‪x& +‬‬ ‫‪∂x‬‬ ‫‪∂t‬‬ ‫= ) ‪y& ( x , t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫& ) ‪∂ y ( x, t‬‬ ‫‪θy ( x , t ) − v ‬‬ ‫‪= −θ& x −‬‬ ‫‪∂t‬‬ ‫‪∂x ‬‬ ‫)‪(2.4.6‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪= −θ& x − θ& y ( x , t‬‬ ‫‪+v‬‬ ‫‪∂t‬‬ ‫‪∂x‬‬ ‫‪∂x‬‬ ‫גזירה של )‪ ,(2.4.6‬פעם ופעמיים לפי ‪:x‬‬ ‫& ‪ ∂ y ( x, t ) ‬‬ ‫) ‪∂2 y ( x, t‬‬ ‫) ‪∂ 2 y ( x, t‬‬ ‫) ‪∂ 2 y ( x, t‬‬ ‫&‬ ‫&‬ ‫‪= −θ− θ ‬‬ ‫‪+v‬‬ ‫) ‪ − θ y ( x, t‬‬ ‫‪∂t ∂x‬‬ ‫‪∂ x2‬‬ ‫‪∂ x2‬‬ ‫‪ ∂x ‬‬ ‫‪2‬‬ ‫)‪(2.4.7‬‬ ‫& ) ‪∂ y ( x, t ) ∂ y ( x, t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫&‪= −3θ‬‬ ‫) ‪− θy ( x , t‬‬ ‫‪+v‬‬ ‫‪2‬‬ ‫‪3‬‬ ‫‪∂x‬‬ ‫‪∂x‬‬ ‫‪∂x‬‬ ‫‪∂ x3‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪3‬‬ ‫‪tan φ‬‬ ‫= &‪) θ‬מתוך מודל כלי הרכב )‪,((1.2‬‬ ‫נציב ב‪ (2.4.6) -‬ו‪v (2.4.7) -‬‬ ‫‪L‬‬ ‫‪80‬‬ ‫) ‪∂ y ( x, t‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪∂t ∂x‬‬ ∂ y ( x , t )  tan φ ∂ y ( x, t )  tan φ   = − x+ 1 − y ( x, t ) v  ∂t ∂x  L    L 2 ∂2 y ( x, t )  ∂ 2 y ( x, t )  tan φ  tan φ tan φ  ∂ y ( x , t )   = 1 − y ( x , t ) L  − L − L  ∂ x   v ∂t ∂x  ∂ x2       ∂3 y ( x , t ) ∂ t ∂ x2 (2.4.8) 2  ∂3 y ( x, t )  tan φ  tan φ ∂ y ( x , t ) ∂ y ( x , t )  = − − , 1 y x t 3 ( ) v  3 ∂x L  L ∂ x 2    ∂ x :‫נגדיר את שגיאות כלי הרכב ביחס למסלול הרצוי באופן הבא‬ e1 ( t ) = y ( x , t ) e2 ( t ) = x =0 ∂ y ( x, t ) ∂ x x =0 ( (2.4.9) )  ∂2 y ( x, t ) 1 + e22 tan φ    e3 ( t ) = −  ∂ x2 L − e1 tan φ  x =0   ‫ שגיאה בכיוון הגלגלים‬e 3 -‫ שגיאת כיוון ו‬e 2 ,‫ שגיאת מקום‬e1 (2.4.9) -‫כאשר ב‬ ‫ הגדרה כזאת מאפשרת טרנספורמציה למודל שרשרת כאשר‬.‫הקדמיים של כלי הרכב‬ . φ < ± π 2 ‫ ( ו‬L − e1 tan φ ) ≠ 0 :‫ ע"י‬x = 0 -‫נסמן את הנגזרת השלישית ב‬ f ′′′ = ∂3 y ( x , t ) ∂ x3 (2.4.10) x =0 :‫( מתקבל‬2.4.10) ‫ ( ושימוש בסימון‬x = 0 ‫( )עבור‬2.4.8) -‫ הצבה ב‬,(2.4.9) ‫מתוך גזירה של‬  L − e1 tan φ  e&1 = e 2  v L    L − e1 tan φ  e& 2 = e3  v L   (2.4.11) ( ) ( )  4e2 1 + e22 tan 2 φ   L − e tan φ  L 1 + e22 5e 2e3 tan φ 1  &e3 =  f ′′′ − − w  v − 2 2 2 L − e1 tan φ ) L   (  φ − φ φ L − e tan L e tan cos ( ) ( ) 1 1   :‫נסמן‬  L − e1 tan φ  v= v L   (2.4.12) :‫ונבחר אות בקרה מהצורה‬ 81 ‫)‪(2.4.13‬‬ ‫(‬ ‫)‬ ‫‪‬‬ ‫‪ cos 2 φ L − e tan φ 2‬‬ ‫‪4e2 1 + e22 tan 2 φ ‬‬ ‫‪tan‬‬ ‫‪5e‬‬ ‫‪e‬‬ ‫‪φ‬‬ ‫(‬ ‫)‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪3‬‬ ‫‪v − u‬‬ ‫‪w =   f ′′′ −‬‬ ‫‪−‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪( L − e1 tan φ ) ( L − e1 tan φ ) ‬‬ ‫‪L 1 + e2‬‬ ‫‪‬‬ ‫‪‬‬ ‫(‬ ‫)‬ ‫מתוך הצבה של )‪ (2.4.13‬ב‪ (2.4.11) -‬מתקבל מודל בצורת שרשרת מסדר שלישי‪.‬‬ ‫‪e& 1 = e 2 v‬‬ ‫‪e& 2 = e 3 v‬‬ ‫)‪(2.4.14‬‬ ‫‪e& 3 = u‬‬ ‫בחירה של אות הבקרה ‪ u‬באופן הבא‪:‬‬ ‫‪σ = a1e1 + a 2e 2 + e3‬‬ ‫)‪(2.4.15‬‬ ‫‪u =  − kh ( σ ) − a1e2 − a 2e3  v , a1 , a 2 > 0‬‬ ‫שימוש במשפטים ‪ 2.1‬ו‪ 2.2-‬וקיום התנאים הבאים‪:‬‬ ‫‪e2 ≤ tan θmax = π 2 − ε1 , ε1 > 0‬‬ ‫‪φ < φmax = π 2 − ε 2 , ε 2 > 0‬‬ ‫)‪(2.4.16‬‬ ‫‪ L − e1 tan φ ‬‬ ‫‪0 < η1 ≤ ‬‬ ‫∞ < ‪ v ≤ η2‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫מבטיחים יציבות אסימפטוטית של הראשית ב‪) (2.4.14) -‬התנאי הראשון הוא תולדה‬ ‫של ההנחה הנתונה באיור ‪ .(2.4.2‬במקרה זה אות הבקרה שיש לספק ל‪ (1.2) -‬נתון ע"י‪:‬‬ ‫)‬ ‫)‪(2.4.17‬‬ ‫(‬ ‫‪4e2 tan 2 φ 1 + e 22‬‬ ‫‪‬‬ ‫‪5e 2e3 tan φ‬‬ ‫‪w =  f ′′′ −‬‬ ‫‪−‬‬ ‫‪‬‬ ‫‪( L − e1 tan φ ) ( L − e1 tan φ )2‬‬ ‫‪‬‬ ‫‪ cos φ ( L − e1 tan φ ) v‬‬ ‫‪+ kh ( σ ) + a1e 2 + a 2e3 ‬‬ ‫‪L2 1 + e22‬‬ ‫‪‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫)‬ ‫(‬ ‫הערות לגבי בקר )‪(2.4.17‬‬ ‫‪ .1‬כדי להבהיר את המקור לדרישה ) ‪ 0 < η1 ≤ v ( t‬נרשום שוב את המשוואות‬ ‫הקינמאטיות של נקודת מסלול הנתונה במערכת כלי הרכב )משוואה )‪ ((2.4.5‬ונציב‬ ‫‪ x = 0‬ו‪: y = e1 -‬‬ ‫)‪(2.4.18‬‬ ‫‪ tan φ‬‬ ‫‪‬‬ ‫‪ tan φ‬‬ ‫‪‬‬ ‫‪x& = ‬‬ ‫‪y − 1 v = ‬‬ ‫‪e1 − 1 v‬‬ ‫‪ L‬‬ ‫‪‬‬ ‫‪ L‬‬ ‫‪‬‬ ‫‪tan φ‬‬ ‫‪y& = −‬‬ ‫‪xv = 0‬‬ ‫‪L‬‬ ‫‪82‬‬ ‫מתוך‬ ‫)‪(2.4.18‬‬ ‫ברור‬ ‫כי‬ ‫‪e1 tan φ = L‬‬ ‫כאשר‬ ‫אז‬ ‫‪, x& = y& = 0‬‬ ‫כלומר‬ ‫המהירות היחסית בין הנקודה בה נמדד המסלול הרצוי וכלי הרכב שווה אפס לכן‬ ‫התכנסות אינה אפשרית‪.‬‬ ‫‪ .2‬האופן שבו מוגדרים כאן משתני המצב ) ‪ e3 ( t‬והפונקציה ) ‪ v ( t‬במודל השרשרת‬ ‫מקשה מאוד על שימוש בחסמים )‪ (2.1.24‬לצורך מציאת תנאים מספיקים המבטיחים‬ ‫יציבות מקומית של הבקר ותנאים כאלה לא נמצאו במסגרת עבודה זו‪.‬‬ ‫למרות העובדה כי הבסיס התיאורטי כאן אינו שלם‪ ,‬סימולציות וניסויים רבים מראים‬ ‫כי שימוש בבקר )‪ (2.4.17‬כאשר תנאי ההתחלה ) ‪ e1 ( 0‬ו‪ φ ( 0 ) -‬קטנים )רצוי לבחור‬ ‫‪ ( φ ( 0 ) = 0‬מאפשרים התכנסות והשגת ביצועים טובים )ראה דוגמא בסוף סעיף זה(‪.‬‬ ‫‪ .3‬נציג כאן בקצרה פיתוח מקביל )לפיתוח בקר )‪ ((2.4.17‬עבור כלי רכב בעל הנעה‬ ‫דיפרנציאלית‪.‬‬ ‫מתוך )‪ (2.4.6‬ו‪ (2.4.7) -‬כאשר ‪ θ& = ω‬מתקבל‪:‬‬ ‫)‪(2.4.19‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪∂ y ( x, t‬‬ ‫) ‪= −ω x − ω y ( x , t‬‬ ‫‪+v‬‬ ‫‪∂t‬‬ ‫‪∂x‬‬ ‫‪∂x‬‬ ‫‪ ∂ y ( x, t ) ‬‬ ‫) ‪∂2 y ( x, t‬‬ ‫) ‪∂2 y ( x, t‬‬ ‫) ‪∂2 y ( x, t‬‬ ‫‪= −ω− ω ‬‬ ‫‪−‬‬ ‫‪ω‬‬ ‫‪+‬‬ ‫‪y‬‬ ‫‪x‬‬ ‫‪,‬‬ ‫‪t‬‬ ‫‪v‬‬ ‫(‬ ‫)‬ ‫‪‬‬ ‫‪∂t ∂x‬‬ ‫‪∂ x2‬‬ ‫‪∂ x2‬‬ ‫‪ ∂x ‬‬ ‫‪2‬‬ ‫) ‪∂ 2 y ( x, t‬‬ ‫שימוש בהגדרות ) ‪ e1 ( t‬ו‪ e2 ( t ) -‬מתוך )‪ ,(2.4.9‬סימון‬ ‫‪x =0‬‬ ‫‪ x = 0‬ב‪ (1.4.19) -‬נותן‪:‬‬ ‫)‪(2.4.20‬‬ ‫)‬ ‫) ‪e&1 = e 2 ( v − ωe1‬‬ ‫(‬ ‫‪e& 2 = f ′′ ( v − ω e1 ) − ω 1 + e22‬‬ ‫שימוש באות בקרה ‪ ω‬מהצורה‪:‬‬ ‫)‪(2.4.21‬‬ ‫‪f ′′ v − u‬‬ ‫‪1 + e 22‬‬ ‫=‪ω‬‬ ‫כאשר‪:‬‬ ‫)‪(2.4.22‬‬ ‫‪v = v − e1ω‬‬ ‫מביא )ע"י הצבה ב‪ (2.4.20) -‬למודל בצורת שרשרת מסדר שני‪.‬‬ ‫‪83‬‬ ‫‪∂ x2‬‬ ‫= ‪ f ′′‬והצבה של‬ ‫‪e&1 = e 2 v‬‬ ‫‪e& 2 = u‬‬ ‫)‪(2.4.23‬‬ ‫כאשר ניתן לקיים את אי השוויון ∞ < ‪) 0 < η1 ≤ v ( t ) ≤ η2‬עבור ) ‪ v ( t‬המוגדר ב‪-‬‬ ‫)‪ ((2.4.22‬אז בחירת ‪ u‬באופן הבא‪:‬‬ ‫‪σ = ae1 + e2‬‬ ‫)‪(2.4.24‬‬ ‫‪u =  − kh ( σ ) − ae2  v , a > 0‬‬ ‫ושימוש במשפטים ‪ 2.1‬ו‪ 2.2-‬מאפשרים יציבות מקומית של הראשית ב‪.(2.4.23) -‬‬ ‫אות הבקרה המתקבל מתוך הצבה של )‪ (2.4.24‬ב‪ (2.4.23) -‬נתון ע"י‪:‬‬ ‫‪f ′′ + kh ( σ ) + ae 2  v‬‬ ‫‪ω= ‬‬ ‫‪1 + e 22‬‬ ‫)‪(2.4.25‬‬ ‫גם כאן המצב הסינגולארי ‪ v ( t ) = v − e1ω = 0‬נובע מאיפוס המהירות )היחסית( בין‬ ‫נקודת הייחוס על המסלול הרצוי וכלי הרכב‪ .‬לכאורה נראה כי ניתן לקיים את הדרישה‬ ‫‪ 0 < η1 ≤ v − e1ω‬ע"י בחירה מתאימה של המהירות ‪ ,v‬אבל‪ ,‬מכיוון שגם אות הבקרה ‪ω‬‬ ‫תלוי ב‪ v -‬נמצא כי רעיון זה שגוי‪ ,‬לכן נציג תנאי אחר אשר אינו מבוסס על המהירות ‪.v‬‬ ‫מתוך הצבה של ‪ v‬ב‪:(2.4.12) -‬‬ ‫)‪f ′′ + kh ( σ ) + ae2  ( v − e1ω‬‬ ‫‪ω= ‬‬ ‫‪1 + e22‬‬ ‫⇒‬ ‫)‪(2.4.26‬‬ ‫‪f ′′ + kh ( σ ) + ae2 ‬‬ ‫‪v‬‬ ‫‪1 + + f ′′ + kh ( σ ) + ae2  e1‬‬ ‫‪e22‬‬ ‫=‪ω‬‬ ‫נציב את )‪ (2.4.26‬ב‪:(2.4.22) -‬‬ ‫)‪(2.4.27‬‬ ‫‪f ′′ + kh ( σ ) + ae2  e1‬‬ ‫‪v‬‬ ‫‪1 + e 22 + f ′′ + kh ( σ ) + ae2  e1‬‬ ‫‪v = v − e1ω = v −‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪1 + e 22‬‬ ‫‪v‬‬ ‫‪=‬‬ ‫‪ 1 + e22 + f ′′ + kh ( σ ) + ae2  e1 ‬‬ ‫‪‬‬ ‫‪ ‬‬ ‫‪‬‬ ‫מכאן ברור כי כדי למנוע איפוס של ) ‪ v ( t‬וסינגולאריות של אות הבקרה ) ‪ ω ( t‬דרוש‪:‬‬ ‫)‪(2.4.28‬‬ ‫‪1 + e 22 + f ′′ + kh ( σ ) + ae2  e1 ≠ 0‬‬ ‫‪84‬‬ ‫לסיכום דיון זה העוסק בפיתוח בקר )‪ (2.4.17‬מוצגות בגרפים ‪ 2.43-2.47‬תוצאות של‬ ‫סימולציה אשר בוצעה עם מודל של כלי רכב בעל ארבעה גלגלים‪ .‬תנאי ההתחלה של‬ ‫כלי הרכב בסימולציה הם‪ , x ( 0 ) = 2 [ m ] , y ( 0 ) = 0, θ ( 0 ) = 90 [ deg ] , φ ( 0 ) = 0 :‬המרחק‬ ‫בין ציר הגלגלים הקדמי והאחורי ]‪, L = 1 [m‬המסלול הרצוי נתון באופן פרמטרי ע"י‬ ‫‪ , f x ( s ) = 5 cos ( s ) , f y ( s ) = 5 sin ( 0.9s ) + s‬המהירות הקווית של כלי הרכב ] ‪v = 1 [ m / s‬‬ ‫ונתוני הבקר הם ) ‪ . k = 2, a1 = 2, a 2 = 4, h ( σ ) = tanh ( σ‬בגרף ‪ 2.47‬ניתן לראות כי‬ ‫הביטוי ) ‪) ( L − e1 tan φ‬ולכן גם ) ‪ ( v ( t‬נשאר חיובי במשך כל הסימולציה‪.‬‬ ‫‪Trajectory‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪8‬‬ ‫‪7‬‬ ‫‪6‬‬ ‫‪5‬‬ ‫‪4‬‬ ‫]‪y [m‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪0‬‬ ‫‪2‬‬ ‫‪-2‬‬ ‫‪-4‬‬ ‫]‪x [m‬‬ ‫גרף ‪ –2.43‬מסלול כלי הרכב )סימולציה עם בקר בקואורדינאטות מקומיות )‪((2.4.17‬‬ ‫‪85‬‬ ‫‪Control Inputs‬‬ ‫‪2‬‬ ‫‪1.5‬‬ ‫)‪v(t‬‬ ‫‪1‬‬ ‫‪0.5‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪2‬‬ ‫‪1.5‬‬ ‫‪1‬‬ ‫)‪w(t‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫‪-1‬‬ ‫גרף ‪ –2.44‬אותות בקרה )סימולציה עם בקר בקואורדינאטות מקומיות )‪((2.4.17‬‬ ‫‪Vehicle Angles‬‬ ‫‪400‬‬ ‫‪300‬‬ ‫]‪θ(t) [deg‬‬ ‫‪200‬‬ ‫‪100‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪30‬‬ ‫‪20‬‬ ‫]‪φ (t) [deg‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪-10‬‬ ‫גרף ‪ –2.45‬זווית כלי הרכב וזווית הגלגלים הקדמיים )סימולציה עם בקר‬ ‫בקואורדינאטות מקומיות )‪((2.4.17‬‬ ‫‪86‬‬ ‫‪Tracking Errors‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫)‪e1(t‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫‪0.2‬‬ ‫‪0‬‬ ‫)‪e2(t‬‬ ‫‪-0.2‬‬ ‫‪-0.4‬‬ ‫‪-0.6‬‬ ‫‪0.2‬‬ ‫‪0‬‬ ‫)‪e3(t‬‬ ‫‪-0.2‬‬ ‫‪-0.4‬‬ ‫‪-0.6‬‬ ‫גרף ‪ –2.46‬שגיאות )סימולציה עם בקר בקואורדינאטות מקומיות )‪((2.4.17‬‬ ‫) ‪(L-e1tanφ‬‬ ‫‪1.1‬‬ ‫‪1‬‬ ‫‪0.9‬‬ ‫‪0.8‬‬ ‫‪0.7‬‬ ‫‪0.6‬‬ ‫‪0.5‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪15‬‬ ‫]‪time [sec‬‬ ‫‪10‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪0.4‬‬ ‫גרף ‪ –2.47‬הביטוי ) ‪) , ( L − e1 tan φ‬סימולציה עם בקר בקואורדינאטות מקומיות‬ ‫)‪.((2.4.17‬‬ ‫‪87‬‬ ‫מערכת קואורדינאטות מקומית נוספת אשר ניתן למדוד בה את שגיאות כלי הרכב‬ ‫ביחס למסלול רצוי )המיוצג ע"י עקום פרמטרי( היא מערכת קואורדינאטות "צמודה‬ ‫למסלול"‪ .‬במערכת זו אשר מסומנת באיור ‪ 2.4.3‬ע"י ) ‪ ( x r , y r‬הראשית נמצאת על‬ ‫המסלול בנקודה ‪ ,s‬ציר ‪ x r‬משיק למסלול בנקודה ‪ s‬וציר ‪ yr‬עובר דרך הראשית של‬ ‫מערכת הצירים ) ‪ ( x , y‬הרתומה לכלי הרכב‪ .‬הנקודה ‪ s‬היא למעשה נקודת המסלול‬ ‫הקרובה ביותר לראשית מערכת הצירים של כלי הרכב‪.‬‬ ‫‪Y‬‬ ‫‪yr‬‬ ‫‪xr‬‬ ‫‪θ‬‬ ‫‪θr‬‬ ‫‪s‬‬ ‫‪x‬‬ ‫‪θ‬‬ ‫‪d‬‬ ‫‪ys‬‬ ‫‪y‬‬ ‫‪v‬‬ ‫‪θr‬‬ ‫‪yc‬‬ ‫‪θr‬‬ ‫‪X‬‬ ‫‪xs‬‬ ‫‪xc‬‬ ‫איור ‪ – 2.4.3‬מערכת קואורדינאטות "צמודה למסלול"‬ ‫האורך ‪ d‬הוא מרחק כלי הרכב מהמסלול הרצוי‪ ,‬ברור כי‪:‬‬ ‫‪2.4.29‬‬ ‫) ‪d& = − v sin ( θ − θr ) = v sin ( θr − θ‬‬ ‫)רצוי להדגיש כאן כי ‪ d‬חיובי כאשר כלי הרכב נמצא מימין למסלול כפי שנתון באיור(‬ ‫עקמומיות המסלול בנקודה ‪ s‬מסומנת ע"י ) ‪ , C ( s‬מכאן‪:‬‬ ‫&‪θ& r = C ( s ) s‬‬ ‫‪2.4.30‬‬ ‫כמו כן‪ ,‬מתוך איור ‪ 2.4.3‬ברור כי‪:‬‬ ‫‪2.4.31‬‬ ‫‪( x c − xs ) cos θr = ( ys − yc ) sin θr‬‬ ‫מתוך גזירה של )‪ (2.4.31‬מתקבל‪:‬‬ ‫‪88‬‬ ( x& c − x& s ) cos θr − ( x c − x s ) sin θr θ& r = ( y& s − y& c ) sin θr + ( ys − yc ) cos θr θ& r ( v cos θ − s& cos θr ) cos θr − ( x c − x s ) sin θr θ& r = ( s& sin θr − v sin θ ) sin θr + ( ys − yc ) cos θr θ& r ⇒ s& = v cos ( θr − θ ) − dθ& r (2.4.32) (2.4.30) -‫( ו‬2.4.32) ‫מתוך‬ s& = cos ( θr − θ ) v 1 + dC ( s ) (2.4.33) C ( s ) cos ( θr − θ ) θ& r = v 1 + dC ( s ) ‫ אינם מוגדרים כאשר‬θ& r -‫ ו‬s& ‫( ניתן לראות כי‬2.4.33) ‫מהשוויוניים האחרונים‬ ‫ כלומר אינם מוגדרים כאשר מרחק כלי הרכב מהמסלול הרצוי שווה‬1 + dC ( s ) = 0 ‫ )מצב זה אנלוגי למצב הסינגולארי‬s ‫לרדיוס העקמומית של המסלול בנקודה‬ .((2.4.36) ‫ במקרה של בקר‬L + e1 tan φ = 0 :‫נגדיר את השגיאות באופן הבא‬ e1 = d e2 = sin ( θr − θ ) (2.4.34) C ( s ) cos 2 ( θr − θ ) tan φ cos ( θr − θ ) − e3 = 1 + dC ( s ) L . dC ( s ) < 1 − ε1 , φ < π 2 − ε2 , ε1 , ε2 > 0 ‫כאשר‬ ‫ שגיאה בכיוון הגלגלים הקדמיים של כלי‬e 3 -‫ שגיאת כיוון ו‬e 2 ,‫ שגיאת מקום‬e1 ‫כאן‬ ‫( תתקבל‬1.2) ‫( ומודל כלי הרכב‬2.4.29) ,(2.4.33) ‫( והצבה של‬2.4.34) ‫ מגזירת‬.‫הרכב‬ :‫המערכת הבאה‬ e&1 = e 2 v e& 2 = e3v e& 3 = C′ ( s ) cos3 ( θr − θ ) +3 (1 + dC ( s ) ) 2 v−3 C 2 ( s ) cos 2 ( θr − θ ) sin ( θr − θ ) (1 + dC ( s ) ) C ( s ) cos ( θr − θ ) sin ( θr − θ ) tan φ L (1 + dC ( s ) ) v− 2 v− sin ( θr − θ ) tan 2 φ dC ( s ) C′ ( s ) cos3 ( θr − θ ) (1 + dC ( s ) ) 3 89 L2 v− v cos ( θr − θ ) L cos 2 φ (2.4.35) w ‫נבחר אות בקרה‬  ′ C ( s ) cos3 ( θr − θ ) C2 ( s ) cos 2 ( θr − θ ) sin ( θr − θ ) sin ( θr − θ ) tan 2 φ   w= −3 − 2 2  L2 1 dC s 1 dC s + + ( ) ( ) ( ) ( )    C ( s ) cos ( θr − θ ) sin ( θr − θ ) tan φ dC ( s ) C′ ( s ) cos ( θr − θ )  L cos 2 φ  +3 − −u 3  cos ( θr − θ )  L (1 + dC ( s ) ) (1 + dC ( s ) )   (2.4.36) 3 θr − θ < π 2 − ε3 , ε3 > 0 ‫כאשר‬ .‫מתוך שתי המשוואות האחרונות מתקבל מודל בצורת שרשרת מסדר שלישי‬ e&1 = e 2 v e& 2 = e3 v (2.4.37) e& 3 = u ,‫ באופן הבא‬u ‫בחירת‬ σ = a1e1 + a 2e2 + e3 u =  − kh ( σ ) − a1e2 − a 2e3  v , a1 , a 2 > 0 dC ( s ) < 1 − ε, (2.4.38) θ − θr , φ < π 2 − ε , ε > 0 ‫ וקיום התנאים‬2.2 -‫ ו‬2.1 ‫שימוש במשפטים‬ ‫ במקרה זה אות הבקרה שיש‬,(2.4.37) -‫מבטיחים יציבות אסימפטוטית של הראשית ב‬ :‫ הוא‬f x ( s ) , f y ( s ) ‫( לצורך עקיבה אחרי המסלול‬1.2) -‫לספק ל‬  C′ s cos3 θ − θ C2 ( s ) cos 2 ( θr − θ ) sin ( θr − θ ) ( ) ( ) r  −3 w= 2  (1 + dC ( s ) )2 (1 + dC ( s ) )  − − sin ( θr − θ ) tan 2 φ 2 L +3 C ( s ) cos ( θr − θ ) sin ( θr − θ ) tan φ dC ( s ) C′ ( s ) cos3 ( θr − θ ) (1 + dC ( s ) ) 3 L (1 + dC ( s ) ) (2.4.39)  L cos 2 φ + kh ( σ ) + a1e2 + a 2e3  v  cos ( θr − θ )  ‫( ברור כי אי השוויוניים הבאים מאפשרים יציבות מקומית של‬2.4.36) -‫( ו‬2.4.34) ‫מתוך‬ .‫החוג הסגור‬ dC ( s ) < 1 − ε1 , ε1 > 0 θr − θ < θmax = π 2 − ε 2 , ε 2 > 0 φ < φmax = π 2 − ε3 , ε3 > 0 90 (2.4.40) ‫נמצא תנאים מספיקים לקיום )‪ (2.4.40‬הקשורים בתנאי ההתחלה של כלי הרכב‬ ‫ובתכונות המסלול הרצוי‪ .‬נניח כי המסלול שאחריו רוצים לעקוב‪ ,‬המהירות הקווית של‬ ‫כלי הרכב וקבועי הבקר ‪ a1‬ו‪ a 2 -‬מקיימים‪:‬‬ ‫∞ < ‪, sup v ( t ) = vmax < ∞ , sup C ( s ) = Cmax‬‬ ‫‪t‬‬ ‫‪s‬‬ ‫)‪(2.4.41‬‬ ‫‪inf v ( t ) = vmin > 0‬‬ ‫‪t‬‬ ‫‪ 0‬‬ ‫‪1  ‬‬ ‫‪sup exp  ‬‬ ‫‪ t  = c3 , c 4 = c3 + 1‬‬ ‫‪a‬‬ ‫‪a‬‬ ‫‪−‬‬ ‫‪−‬‬ ‫‪t‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫במודל השרשרת )‪ v ( t ) = v ( t ) (2.4.37‬לכן‬ ‫‪, η2 = vmax‬‬ ‫)‪(2.4.42‬‬ ‫‪η1 = vmin‬‬ ‫מתוך ההגדרה של ‪ e1‬ב‪ ,(2.4.34) -‬שימוש בחסם )‪ (2.1.23‬והצבה של )‪:(2.4.41‬‬ ‫)‪(2.4.43‬‬ ‫‪‬‬ ‫‪ Cmax‬‬ ‫‪‬‬ ‫‪‬‬ ‫)‪ e (0‬‬ ‫) ‪a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0‬‬ ‫‪dC ( s ) = e1 C ( s ) ≤  c3 1‬‬ ‫‪+ c4‬‬ ‫) ‪ e2 ( 0‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫מתוך )‪ (2.4.43‬תנאי מספיק לקיום אי השוויון הראשון ב‪ (2.4.40) -‬נתון ע"י‪:‬‬ ‫)‪(2.4.44‬‬ ‫‪‬‬ ‫‪ Cmax < 1 − ε1 , ε1 > 0‬‬ ‫‪‬‬ ‫‪‬‬ ‫) ‪ e1 ( 0‬‬ ‫) ‪a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0‬‬ ‫‪+ c4‬‬ ‫‪ c3‬‬ ‫) ‪ e2 ( 0‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫מתוך ההגדרה של ‪ e2‬ב‪ ,(2.4.34) -‬שימוש בחסם )‪ (2.1.23‬והצבה של )‪:(2.4.41‬‬ ‫)‪(2.4.45‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫)‪ e (0‬‬ ‫) ‪a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0‬‬ ‫‪e2 = sin ( θr − θ ) <  c3 1‬‬ ‫‪+ c4‬‬ ‫) ‪ e2 ( 0‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫כדי לקיים את הדרישה השנייה ב‪ (2.4.40) -‬דרוש‪:‬‬ ‫)‪(2.4.46‬‬ ‫‪‬‬ ‫) ‪ < sin ( θmax‬‬ ‫‪‬‬ ‫‪‬‬ ‫) ‪ e1 ( 0‬‬ ‫) ‪a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0‬‬ ‫‪+ c4‬‬ ‫‪ c3‬‬ ‫) ‪ e2 ( 0‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫מתוך ההגדרה של ‪ e3‬ב‪ ,(2.4.34) -‬שימוש בחסם )‪ (2.1.23‬והצבה של )‪:(2.3.41‬‬ ‫‪e3‬‬ ‫) ‪tan φ C ( s ) cos ( θr − θ‬‬ ‫⇒‬ ‫=‬ ‫‪−‬‬ ‫‪L‬‬ ‫) ‪1 + dC ( s‬‬ ‫) ‪cos ( θr − θ‬‬ ‫)‪(2.4.47‬‬ ‫)‪C (s‬‬ ‫‪e3‬‬ ‫) ‪tan φ C ( s ) cos ( θr − θ‬‬ ‫≤‬ ‫‪+‬‬ ‫≤‬ ‫‪L‬‬ ‫) ‪1 + dC ( s‬‬ ‫) ‪cos ( θr − θ ) 1 + dC ( s‬‬ ‫‪ c a2 + a2‬‬ ‫‪‬‬ ‫)‪e (0‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪ 4 1‬‬ ‫‪+ 1 a1e1 ( 0 ) + a 2e2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1‬‬ ‫) ‪e2 ( 0‬‬ ‫‪a1‬‬ ‫‪cos ( θr − θ ) ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪91‬‬ ‫‪+‬‬ ‫נניח כי )‪ (2.4.44‬ו‪ (2.4.46) -‬מתקיימים‪ ,‬מכאן תנאי מספיק לקיום אי השוויון‬ ‫‪) tan φ < tan φmax‬ראה ‪ (2.4.21‬נתון ע"י‪:‬‬ ‫)‪(2.4.48‬‬ ‫‪ c a 2 + a 2‬‬ ‫‪‬‬ ‫‪e ( 0) ‬‬ ‫‪Cmax‬‬ ‫‪1‬‬ ‫‪2‬‬ ‫‪ 4 1‬‬ ‫‪‬‬ ‫‪+‬‬ ‫‪+ 1 a1e1 ( 0 ) + a 2e 2 ( 0 ) + e3 ( 0 ) + c3 a12 + a 22 1‬‬ ‫‪e2 ( 0 ) ‬‬ ‫‪ε1‬‬ ‫‪cos θmax ‬‬ ‫‪‬‬ ‫‪a1‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪tan φmax‬‬ ‫<‬ ‫‪L‬‬ ‫)קיום הדרישה השלישית ב‪ (2.4.40) -‬מחייב כאן גם קיום הדרישות הראשונה והשנייה(‪.‬‬ ‫בגרפים ‪ 2.48-2.52‬מוצגות תוצאות של סימולציה אשר בוצעה עם מודל של כלי רכב‬ ‫בעל ארבעה גלגלים ובקר היגוי בקואורדינאטות מסלול )‪ .(2.4.39‬תנאי ההתחלה של כלי‬ ‫הרכב בסימולציות הם‪ , x ( 0 ) = 2 [ m ] , y ( 0 ) = 0, θ ( 0 ) = 90 [ deg ] , φ ( 0 ) = 0 :‬המרחק בין‬ ‫ציר הגלגלים הקדמי והאחורי ]‪, L = 1 [m‬המסלול הרצוי נתון באופן פרמטרי ע"י‬ ‫) ‪ , f x ( s ) = 5 cos ( 0.2s ) , f y ( s ) = 5 sin ( 0.2s‬המהירות הקווית של כלי הרכב ] ‪v = 1 [ m / s‬‬ ‫ונתוני הבקר הם ) ‪ . k = 2, a1 = 2, a 2 = 4, h ( σ ) = tanh ( σ‬בגרף ‪ 2.52‬ניתן לראות כי‬ ‫הביטוי ) ) ‪ (1 + dC ( s‬נשאר חיובי במשך כל הסימולציה‪.‬‬ ‫‪Trajectory‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪6‬‬ ‫‪5‬‬ ‫‪4‬‬ ‫‪3‬‬ ‫]‪y [m‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫‪5‬‬ ‫‪4‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫]‪x [m‬‬ ‫‪-1‬‬ ‫‪-2‬‬ ‫‪-3‬‬ ‫‪-4‬‬ ‫‪-5‬‬ ‫גרף ‪ –2.48‬מסלול כלי הרכב )סימולציה עם בקר בקואורדינאטות מסלול )‪((2.4.39‬‬ ‫‪92‬‬ ‫‪Control Inputs‬‬ ‫‪2‬‬ ‫‪1.5‬‬ ‫)‪v(t‬‬ ‫‪1‬‬ ‫‪0.5‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫)‪w(t‬‬ ‫‪-0.5‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ –2.49‬אותות בקרה )סימולציה עם בקר בקואורדינאטות מסלול )‪((2.4.39‬‬ ‫‪Vehicle Angles‬‬ ‫‪300‬‬ ‫‪250‬‬ ‫‪150‬‬ ‫]‪θ(t) [deg‬‬ ‫‪200‬‬ ‫‪100‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪50‬‬ ‫]‪time [sec‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪10‬‬ ‫]‪φ (t) [deg‬‬ ‫‪15‬‬ ‫‪5‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ –2.50‬זווית כלי הרכב וזווית הגלגלים הקדמיים )סימולציה עם בקר‬ ‫בקואורדינאטות מסלול )‪((2.4.39‬‬ ‫‪93‬‬ ‫‪Tracking Errors‬‬ ‫‪0‬‬ ‫‪-1‬‬ ‫)‪e1(t‬‬ ‫‪-2‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-3‬‬ ‫]‪time [sec‬‬ ‫‪0.8‬‬ ‫‪0.6‬‬ ‫)‪e2(t‬‬ ‫‪0.4‬‬ ‫‪0.2‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫‪0.5‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫)‪e3(t‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ –2.51‬שגיאות עקיבה )סימולציה עם בקר בקואורדינאטות מסלול )‪((2.4.39‬‬ ‫) )‪( 1 + dC(s‬‬ ‫‪1‬‬ ‫‪0.9‬‬ ‫‪0.8‬‬ ‫‪0.7‬‬ ‫‪0.6‬‬ ‫‪0.5‬‬ ‫‪0.4‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫‪6‬‬ ‫‪8‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫]‪time [sec‬‬ ‫גרף ‪ –2.52‬הביטוי ) ) ‪) , (1 + dC ( s‬סימולציה עם בקר בקואורדינאטות מסלול )‪.((2.4.39‬‬ ‫‪94‬‬ ‫‪ – 2.5‬תכנון בקרים לכלי רכב אוטונומיים בגישה האינווריאנטית‬ ‫בסעיף זה מוצגת גישה כללית )וייחודית( לפיה מפותח בקר )לכלי רכב בעל ארבעה‬ ‫גלגלים( המבוסס על גדלים שאינם תלויים בבחירה של מערכת צירים‪ .‬גישה זאת תקרא‬ ‫הגישה האינווריאנטית לתכנון בקרים‪ .‬הרעיון מאחורי פיתוח הבקר בסעיף זה נועד‬ ‫לתת מסגרת אחת לכל הבקרים שהוצגו בסעיפים הקודמים )ואשר פיתוחם התבסס על‬ ‫שימוש במערכות צירים שונות(‪ .‬ניתן ע"י שימוש בגישה המוצעת לפתח בקר כללי‬ ‫מאוד אשר כולל בתוכו את כל הבקרים הקודמים כמקרים פרטיים ומהווה קרקע‬ ‫לפיתוח בקרים נוספים )המבוססים על טרנספורמציה למודל שרשרת(‪.‬‬ ‫לצורך הפיתוח נעשה שימוש במערכת קואורדינאטות קרטזית קבועה במרחב )ראה‬ ‫איור ‪ ,(2.5.1‬אבל התוצאה המתקבלת מבוססת אך ורק על גדלים שאינם תלויים‬ ‫בבחירה של מערכת צירים )יוצאת מכלל זה‪ ,‬זווית הגלגלים הקדמיים ‪ φ‬אשר מוגדרת‬ ‫באיור ‪ 1.2‬ונמדדת ביחס למסגרת כלי הרכב(‬ ‫‪ψ‬‬ ‫‪θr‬‬ ‫‪s‬‬ ‫‪ys‬‬ ‫‪r‬‬ ‫‪d‬‬ ‫‪ψ θ‬‬ ‫‪v‬‬ ‫‪θ − θr − ψ‬‬ ‫‪θr‬‬ ‫‪90 − θ‬‬ ‫‪xc‬‬ ‫‪yc‬‬ ‫‪xs‬‬ ‫איור ‪ – 2.5.1‬פיתוח בקר המבוסס על גדלים אינווריאנטיים‬ ‫המסלול הרצוי נמדד בנקודה ‪ ,s‬ושגיאת המקום הינה המרחק ‪ d‬בין כלי הרכב ובין‬ ‫‪r‬‬ ‫הנקודה ‪) s‬השגיאה ‪ d‬חיובית כאשר כלי הרכב נמצא מימין למסלול(‪ .‬הווקטור ‪ d‬וכיוון‬ ‫המסלול הרצוי בנקודה ‪ s‬משמשים לצורך הגדרת הזווית ‪ ψ‬כפי שנתון באיור ‪2.5.1‬‬ ‫‪r‬‬ ‫)הזווית ‪ ψ‬הינה הפרש הזוויות בין כיוון המסלול ‪ θr‬ובין קו ניצב לווקטור ‪.( d‬‬ ‫‪95‬‬ ‫אם מעוניינים לפתור את בעיית ה‪ trajectory tracking -‬אז יש לבחור את המשתנה &‪s‬‬ ‫)באופן בלתי תלוי ב‪ (v -‬כך שיקיים את אילוצי הזמן של המסלול הרצוי‪ ,‬ואת המהירות‬ ‫הקווית של כלי הרכב ‪ v‬כפונקציה של &‪ . s‬במקרה זה יתקבל פתרון לבעיית ה‪trajectory -‬‬ ‫‪ tracking‬בגישה המבוססת על רכב יחוס‪ ,‬אנו לא נדון כאן במקרה זה‪ .‬אם המשתנה ‪v‬‬ ‫הינו בלתי תלוי ו‪ s& -‬הינו פונקציה של ‪) v‬פונקציה זו קובעת את הזווית ‪ ( ψ‬אז יתקבל‬ ‫פתרון לבעיית ה‪.path following -‬‬ ‫מתוך השרטוט ברור כי‪:‬‬ ‫) ‪d& = −s& sin ψ − v sin ( θ − θr − ψ‬‬ ‫)‪(2.5.1‬‬ ‫כדי שתתאפשר טרנספורמציה למודל בצורת שרשרת יש לבחור &‪ s‬כך ש‪:‬‬ ‫‪s& = q1 ( t ) q 2 ( t ) v‬‬ ‫)‪(2.5.2‬‬ ‫כאשר ‪ , inf q 2 ( t ) = ε , ε > 0‬ו‪ q1 ( t ) -‬אינה תלויה בזווית הגלגלים הקדמיים ‪. φ‬‬ ‫‪t‬‬ ‫הפונקציות ) ‪ q1 ( t‬ו‪ q 2 ( t ) -‬אשר נתונות לבחירה של מתכנן מערכת הבקרה קובעות את‬ ‫הנקודה בה נמדד המסלול הרצוי )בחירות שונות של פונקציות אלה יובילו לפיתוח‬ ‫בקרים מסוגים שונים(‪.‬‬ ‫מתוך הצבה של )‪ (2.5.2‬ב‪ (2.5.1) -‬מתקבל‪:‬‬ ‫)‪(2.5.3‬‬ ‫‪‬‬ ‫‪sin ( θ − θr − ψ ) ‬‬ ‫‪d& = −  q1 ( t ) sin ψ +‬‬ ‫‪ q 2 ( t ) v‬‬ ‫‪‬‬ ‫) ‪q2 ( t‬‬ ‫‪‬‬ ‫‪‬‬ ‫נשתמש בהגדרות הבאות עבור שגיאת מקום‪ ,‬שגיאה בכיוון כלי הרכב והפונקציה‬ ‫)‪: v(t‬‬ ‫‪e1 = d‬‬ ‫)‪(2.5.4‬‬ ‫‪‬‬ ‫‪sin ( θ − θr − ψ ) ‬‬ ‫‪e 2 = −  q1 ( t ) sin ψ +‬‬ ‫‪‬‬ ‫‪‬‬ ‫) ‪q2 ( t‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪v ( t ) = q2 ( t ) v‬‬ ‫מתוך הצבה של )‪ (2.5.4‬ב‪:(2.5.3) -‬‬ ‫)‪(2.5.5‬‬ ‫) ‪e&1 = e2 v ( t‬‬ ‫לצורך המחשה של השיטה‪ ,‬נציג בשלב זה את המקרים הפרטיים הבאים‪:‬‬ ‫‪ .1‬עבור ‪ ψ = 0‬מתקבל מתוך איור ‪ 2.5.1‬ומשוואה )‪(2.5.4‬‬ ‫‪96‬‬ ‫⇒ ) ‪s& + dC ( s ) s& = v cos ( θr − θ‬‬ ‫‪, q2 ( t ) = 1‬‬ ‫)‪(2.5.6‬‬ ‫) ‪cos ( θr − θ‬‬ ‫) ‪cos ( θr − θ‬‬ ‫= ) ‪v , q1 ( t‬‬ ‫) ‪1 + dC ( s‬‬ ‫) ‪1 + dC ( s‬‬ ‫= &‪s‬‬ ‫⇒‬ ‫‪‬‬ ‫‪sin ( θ − θr − ψ ) ‬‬ ‫‪e1 = d , e 2 = −  q1 ( t ) sin ψ +‬‬ ‫‪ = sin ( θr − θ ) , v ( t ) = v‬‬ ‫‪‬‬ ‫‪q‬‬ ‫‪t‬‬ ‫(‬ ‫)‬ ‫‪2‬‬ ‫‪‬‬ ‫‪‬‬ ‫כלומר בחירה של ) ‪ q1 ( t‬ו‪ q 2 ( t ) -‬לפי )‪ (2.5.6‬מובילה לבקר )‪) (2.4.39‬בקר המתואר‬ ‫באמצעות מערכת קואורדינאטות "צמודה למסלול"(‪ .‬שים לב כי עבור ‪ ψ = 0‬משוואה‬ ‫)‪ (2.5.1‬זהה למשוואה )‪.(2.4.29‬‬ ‫‪ .2‬עבור ‪ ψ = θ − θr‬מתקבל מתוך איור ‪ 2.5.1‬ומשוואה )‪(2.5.4‬‬ ‫&‪v − dθ‬‬ ‫‪1‬‬ ‫‪tan φ ‬‬ ‫‪‬‬ ‫‪1− d‬‬ ‫‪v‬‬ ‫=‬ ‫‪‬‬ ‫‪L ‬‬ ‫‪cos ( θr − θ ) cos ( θr − θ ) ‬‬ ‫‪ L − d tan φ ‬‬ ‫‪, q2 ( t ) = ‬‬ ‫‪‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪1‬‬ ‫) ‪cos ( θr − θ‬‬ ‫= &‪s‬‬ ‫= ) ‪q1 ( t‬‬ ‫⇒‬ ‫)‪(2.5.7‬‬ ‫‪‬‬ ‫) ‪sin ( θ − θr − ψ )  sin ( θr − θ‬‬ ‫) ‪= tan ( θr − θ‬‬ ‫‪e1 = d , e2 = −  q1 ( t ) sin ψ +‬‬ ‫= ‪‬‬ ‫‪‬‬ ‫) ‪q2 ( t‬‬ ‫‪‬‬ ‫) ‪ cos ( θr − θ‬‬ ‫‪ L − e1 tan φ ‬‬ ‫‪v ( t ) = q2 ( t ) v = ‬‬ ‫‪v‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫כלומר בחירה של ) ‪ q1 ( t‬ו‪ q 2 ( t ) -‬לפי )‪ (2.5.7‬מובילה לבקר )‪) (2.4.17‬בקר המתואר‬ ‫באמצעות במערכת קואורדינאטות מקומית הרתומה לכלי הרכב(‪.‬‬ ‫כדי לפשט את המשך הפיתוח ומתוך שיקולים הקשורים בביצועי מערכת הבקרה )ונדונו‬ ‫בסעיף ‪ (2.2‬נפגע מעט בכלליות של הבקר ונבחר ‪ . q 2 ( t ) = 1‬במקרה זה התוצאה הסופית‬ ‫תכסה )כמקרים פרטיים( רק את הבקרים שבמהלך פיתוחם מתקבל מודל שרשרת עם‬ ‫) ‪) v ( t ) = v ( t‬לדוגמא בקרים )‪ (2.2.27‬ו‪.((2.4.39) -‬‬ ‫נרשום שוב את משוואה )‪ (2.5.4‬עבור ‪, q 2 ( t ) = 1‬‬ ‫‪97‬‬ ‫‪e1 = d‬‬ ‫) ) ‪e 2 = − ( q1 ( t ) sin ψ + sin ( θ − θr − ψ‬‬ ‫)‪(2.5.8‬‬ ‫‪v(t) = v‬‬ ‫המשך הפתוח בגישה המבוססת על טרנספורמציה למודל בצורת שרשרת מצריך בחירה‬ ‫של פונקציה ) ‪ q1 ( t‬בעלת התכונות הבאות‪:‬‬ ‫)‪(2.5.9‬‬ ‫‪w‬‬ ‫) ‪q7 ( t‬‬ ‫‪L cos 2 φ‬‬ ‫‪q& 1 ( t ) = q 3 ( t ) v , q& 3 ( t ) = q5 ( t ) v +‬‬ ‫ניתן להראות כי בחירה של ) ‪ q1 ( t‬לפי )‪ (2.5.9‬מאפשרת את ההגדרות הבאות‪:‬‬ ‫‪ψ& ( t ) = q 9 ( t ) v , q& 9 ( t ) = q11 ( t ) v‬‬ ‫)‪(2.5.10‬‬ ‫מתוך גזירה של ) ‪ e2 ( t‬ב‪ (2.5.8) -‬מתקבל‪:‬‬ ‫)‬ ‫)‪(2.5.11‬‬ ‫(‬ ‫&‪e& 2 = −q& 1 ( t ) sin ψ − q1 ( t ) cos ψψ& − cos ( θ − θr − ψ ) θ& − θ& r − ψ‬‬ ‫‪‬‬ ‫‪ tan φ‬‬ ‫‪‬‬ ‫‪=  −q 3 ( t ) sin ψ − q1 ( t ) q 9 ( t ) cos ψ − cos ( θ − θr − ψ ) ‬‬ ‫‪− C ( s ) q1 ( t ) − q 9 ( t )   v‬‬ ‫‪ L‬‬ ‫‪‬‬ ‫‪‬‬ ‫מתוך )‪ (2.5.11‬נגדיר ) ‪) e3 ( t‬שגיאה בכיוון הגלגלים הקדמיים של כלי הרכב(‬ ‫)‪(2.5.12‬‬ ‫‪ tan φ‬‬ ‫‪‬‬ ‫‪e3 = −q 3 ( t ) sin ψ − q1 ( t ) q 9 ( t ) cos ψ − cos ( θ − θr − ψ ) ‬‬ ‫‪− C ( s ) q1 ( t ) − q 9 ( t ) ‬‬ ‫‪ L‬‬ ‫‪‬‬ ‫מתוך גזירה של )‪:(2.5.12‬‬ ‫)‬ ‫(‬ ‫‪e& 3 = − sin ψ q 5 ( t ) + q1 ( t ) q 92 ( t ) v − cos ψ ( 2q 3 ( t ) q9 ( t ) + q1 ( t ) q11 ( t ) ) v‬‬ ‫)‬ ‫(‬ ‫‪+ cos ( θ − θr − ψ ) C′ ( s ) q12 ( t ) + C ( s ) q 3 ( t ) + q11 ( t ) v‬‬ ‫)‪(2.5.13‬‬ ‫‪2‬‬ ‫‪ tan φ‬‬ ‫‪‬‬ ‫‪+ sin ( θ − θr − ψ ) ‬‬ ‫‪− C ( s ) q1 ( t ) − q 9 ( t )  v‬‬ ‫‪ L‬‬ ‫‪‬‬ ‫‪ q ( t ) sin ψ + cos ( θ − θr − ψ ) ‬‬ ‫‪− 7‬‬ ‫‪w‬‬ ‫‪L cos 2 φ‬‬ ‫‪‬‬ ‫‪‬‬ ‫כאשר מתוך )‪ (2.5.13‬ומודל כלי הרכב ברור כי יש לדרוש ‪. φ ≤ π 2 − ε1 , ε1 > 0‬‬ ‫שימוש באות בקרה מתאים המאפשר תכנון בגישה של מודל שרשרת מצריך גם‪:‬‬ ‫)‪(2.5.14‬‬ ‫‪, ε2 > 0‬‬ ‫‪q 7 ( t ) sin ψ + cos ( θ − θr − ψ ) > ε 2‬‬ ‫‪98‬‬ ‫( ושימוש באות בקרה‬2.5.12) ,(2.5.8) ‫אם שני התנאים האחרונים מתקיימים אז מתוך‬ :‫מהצורה‬  w =   − sin ψ q 5 ( t ) + q1 ( t ) q 92 ( t ) − cos ψ ( 2q 3 ( t ) q 9 ( t ) + q1 ( t ) q11 ( t ) )  ( ) ( + cos ( θ − θr − ψ ) C′ ( s ) q12 ( t ) + C ( s ) q 3 ( t ) + q11 ( t ) ) (2.5.15) 2  L cos 2 φ  tan φ    + sin ( θ − θr − ψ )  − C ( s ) q1 ( t ) − q 9 ( t )   v − u  q t sin ψ + cos ( θ − θr − ψ )  L    7( ) ‫מתקבל מודל בצורת שרשרת מסדר שלישי‬ e&1 = e 2 v e& 2 = e3 v (2.5.16) e& 3 = u :‫( גם כאן‬2.5.16) ‫כמו בכל הבקרים האחרים שהתבססו על טרנספורמציה למודל‬ σ = a1e1 + a 2e2 + e3 (2.5.17) u =  − kh ( σ ) − a1e2 − a 2e3  v , a1 , a 2 > 0 :‫דוגמא‬ -‫ ו‬q1 ( t ) , q3 ( t ) , q5 ( t ) , q 7 ( t ) , q9 ( t ) ‫ ע"י בחירה מתאימה של הפונקציות‬,‫נראה כיצד‬ .(2.2.27) ‫( את הבקר‬2.5.15) ‫ ניתן לקבל מתוך הבקר הכללי‬, q11 ( t ) :‫במקרה של מערכת קואורדינאטות קרטזית קבועה במרחב‬ ψ = −θr , s& = cos θ v ⇒ cos θr q1 ( t ) = cos θ cos θr (2.5.18) (2.5.18) ‫מתוך‬ q& 1 ( t ) = ⇒ − sin θ cos θr θ& + cos θ sin θr θ& r sin θ tan φ cos 2 θ tan θr = − v + C (s ) v L cos θr cos 2 θr cos 2 θr sin θ tan φ cos 2 θ tan θr q3 ( t ) = − + C (s ) L cos θr cos 2 θr 99 (2.5.19) q& 3 ( t ) = cos θ sin θ tan θr tan φ cos3 θ 2 cos θ tan 2 φ + − C s v C s v v ( ) ( ) cos5 θr L cos2 θr L2 cos θr + ( cos3 θ tan θr 2 tan θr C2 ( s ) + C′ ( s ) cos θr 3 q5 ( t ) = ⇒ ( cos3 θ tan θr 2 tan θr C2 ( s ) + C′ ( s ) ) cos θr 3 sin θ cos θr q7 ( t ) = − cos θ & = −θ& r = −C ( s ) s& = −C ( s ) ψ v cos θr ⇒ sin θ φ& L cos θr cos 2 φ cos θ sin θ tan θr tan φ cos3 θ 2 cos θ tan 2 φ C s C s + − ( ) ( ) L cos 2 θr cos5 θr L2 cos θr + q& 9 ( t ) = − )v− ( cos 2 θ C′ ( s ) + tan θr C2 ( s ) cos θ cos θr ) v + C (s ) sin θ tan φ v L cos θr cos θr 2 q11 ( t ) = − q 9 ( t ) = −C ( s ) ⇒ ( cos 2 θ C′ ( s ) + tan θr C2 ( s ) ) + C (s ) sin θ tan φ L cos θr cos 2 θr :‫ מכאן‬yd = f ( x ) ‫( המסלול הרצוי נתון ע"י פונקציה מהצורה‬2.2.27) ‫במקרה של בקר‬ C (s ) = C ( x ) = f ′′ ( x ) (1 + f ′ ( x )) e1 = d = f ( x ) − y 2 32 = f ′′ ( x )  sin θr  1 +  2  cos θr  e2 = − ( q1 ( t ) sin ψ + sin ( θ − θr − ψ ) ) = 2 32 = f ′′ ( x ) cos3 θr cos θ sin θr − sin θ = f ′ ( x ) cos θ − sin θ cos θr (2.5.20)  tan φ  e3 = −q 3 ( t ) sin ψ − q1 ( t ) q 9 ( t ) cos ψ − cos ( θ − θr − ψ )  − C ( s ) q1 ( t ) − q 9 ( t )   L  tan φ = f ′′ ( x ) cos 2 θ − ( f ′ ( x ) sin θ + cos θ) L .(2.2.20) -‫(( זהה להגדרת השגיאות ב‬2.5.12) -‫( ו‬2.5.8) ‫והגדרת השגיאות כאן )משוואות‬ 100 ‫‪ – 3‬פיתוח בקר לכלי רכב בעל ארבעה גלגלים המבוסס על מודל עם‬ ‫זוויות החלקה‬ ‫כל הבקרים שהוצגו עד כה במסגרת עבודה זו לכלי רכב בעל ‪ 4‬גלגלים פותחו כאשר‬ ‫נעשה שימוש במודל קינמאטי אשר מבוסס על הנחה של "גלגול ללא החלקה"‪.‬‬ ‫זווית החלקה של גלגל מסומנת ע"י ) ‪ α w ( t‬ומוגדרת באיור ‪ ,3.1‬כאשר ) ‪v w ( t‬‬ ‫המהירות הקווית של הגלגל‪.‬‬ ‫) ‪vw ( t‬‬ ‫) ‪αw ( t‬‬ ‫) ‪Fy ( t‬‬ ‫איור ‪ – 3.1‬זווית החלקה של גלגל‬ ‫במקרה של גלגול ללא החלקה‪ . α w ( t ) = 0, ∀ t ,‬למעשה בכלי רכב מהסוג הנדון‪,‬‬ ‫כאשר מסלול הנסיעה אינו קו ישר‪ ,‬קיימות תמיד זוויות החלקה אשר גודלן יחסי‬ ‫לכוח הצידי )‪ (Lateral‬הפועל על הגלגל )ראה ]‪ .([Gillespie 1992] ,[Wong 2001‬זוויות‬ ‫אלה עשויות להיות בעלות משקל בתנועת כלי הרכב כאשר הוא מבצע תמרונים‬ ‫במהירויות גבוהות )תמרונים המצריכים כוח צידי(‪ .‬עבור זוויות החלקה קטנות )עד‬ ‫‪ 5‬מעלות( נהוג להשתמש בקשר הבא‪:‬‬ ‫) ‪ , Fy ( t ) = Cα α w ( t‬כאשר המקדם ‪ Cα‬הינו‬ ‫קבוע ונקרא ‪ .Cornering stiffness‬עבור זוויות החלקה גדולות יותר‪ Cα ,‬אינו קבוע‬ ‫ותלוי בעיקר בכוח הנורמאלי הפועל על הגלגל )כוח ניצב למישור השרטוט(‪.‬‬ ‫במהירויות נסיעה נמוכות כאשר זוויות ההחלקה קטנות ניתן לעשות שימוש בהנחה‬ ‫של "גלגול ללא החלקה"‪ .‬טענה זו מבוססת על הביצועים הטובים אשר התקבלו‬ ‫מהפעלת הבקרים אשר הוצגו עד כה בעבודה זו במסגרת של ניסויים וסימולציות‬ ‫)המבוססות על שימוש במודל דינאמי(‪.‬‬ ‫כאשר כלי הרכב נדרש לבצע תמרונים במהירויות גבוהות אשר מצריכים זוויות‬ ‫החלקה גדולות לא ניתן עוד לעשות שימוש בהנחה של "גלגול ללא החלקה"‬ ‫והתכנון מצריך שימוש במודל אשר כולל בתוכו גם זוויות החלקה‪ .‬המודל המקובל‬ ‫לשימוש במקרה זה הינו מודל דינאמי‪ ,‬אולם בגלל היותו מורכב ולא ליניארי רוב‬ ‫הבקרים אשר פותחו עד כה והתבססו על מודל דינאמי הניחו זוויות היגוי קטנות‪,‬‬ ‫הנחה אשר מאפשרת ליניאריזציה ותכנון בקר במסגרת של בקרה ליניארית‪ .‬בגלל‬ ‫‪101‬‬ ‫העובדה שמודלים קינמטים של כלי רכב ניתנים לייצוג בצורת שרשרת או בצורות‬ ‫הקרובות לצורת שרשרת ועל ידי כך מאפשרים תכנון נוח יחסית של מערכות בקרה‬ ‫פותח בעבודה זו בקר המבוסס על מודל קינמאטי הכולל גם זוויות החלקה ואינו‬ ‫מבוסס על זוויות היגוי קטנות‪.‬‬ ‫בסעיף ‪ 3.1‬של פרק זה מוצג מודל קינמאטי של כלי רכב בעל ארבעה גלגלים הנע על‬ ‫מישור וכולל זוויות החלקה‪ α1 ,‬בגלגלים האחוריים ו‪ α 2 -‬בגלגלים הקדמיים‪ .‬מודל‬ ‫זה משמש בסיס לפיתוח מודל שגיאות של כלי הרכב ביחס למסלול הרצוי כאשר‬ ‫הגדרת השגיאות כאן זהה להגדרת השגיאות בפיתוח בקר )‪ .(2.2.27‬מודל השגיאות‬ ‫המתקבל הינו בעל חלק אחד זהה למודל השרשרת שנדון בפרקים קודמים )של‬ ‫עבודה זו( וחלק נוסף אשר תלוי בזוויות ההחלקה ‪ α1‬ו‪ α 2 -‬ומהווה "הפרעה"‪ .‬ע"י‬ ‫לינאריזציה של מודל השגיאות סביב המסלול הרצוי וסביב הנקודה ‪α1 = α 2 = 0‬‬ ‫מתקבל מודל ליניארי תלוי בזמן מהצורה הבאה‪:‬‬ ‫) ‪e& ( t ) = Av ( t ) e ( t ) + B2 u ( t ) + B1 ( t ) v ( t ) d α ( t‬‬ ‫‪0 1 0‬‬ ‫‪0‬‬ ‫‪α ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪A =  0 0 1  , B2 =  0  , d α ( t ) =  1 ‬‬ ‫‪α 2 ‬‬ ‫‪ 0 0 0 ‬‬ ‫‪1 ‬‬ ‫בסעיף ‪ 3.3‬מוצג פיתוח בקר ) ‪ u ( t‬אשר תפקידו למזער את השפעת ההפרעה ) ‪d ( t‬‬ ‫על השגיאות של כלי הרכב ביחס למסלול הרצוי‪ .‬תכנון הבקר במקרה זה מבוסס על‬ ‫עקרונות מתוך בקרת ‪ H infinity‬ופתרון ‪) LMIs‬אי שיוויונים מטריציונים ליניאריים(‪.‬‬ ‫הבקר המוצג בפרק זה נבחן בתנאי סימולציה עם מודל דינאמי מול בקר אחר‬ ‫המספק ביצועים דומים כאשר מופעל עם מודל קינמטי )ללא החלקות(‪ .‬בהשוואה זו‬ ‫נעשה שימוש בשני מודלים דינאמיים‪ ,‬הראשון מתקבל מתוך תוכנת ‪CarSim‬‬ ‫המיועדת לצרכי תכנון ואנליזה בתעשיית כלי הרכב ואילו השני מבוסס על מודל‬ ‫דינאמי מסוג ‪ Bicycle‬אשר מוצג בעבודה זו‪ .‬בשני המקרים מתקבל יתרון ברור לבקר‬ ‫אשר תכנונו מבוסס על מודל קינמטי הכולל זוויות החלקה‪.‬‬ ‫ככול שידוע למחבר‪ ,‬גישה זו לפתרון בעיית ה‪ Path following -‬בכלי רכב בעל ארבעה‬ ‫גלגלים ע"י שימוש במודל קינמטי אשר אינו מבוסס על הנחה של גלגול ללא‬ ‫החלקה‪ ,‬ופיתוח בקר על פי קריטריון הממזער את ההשפעה של ההחלקה על‬ ‫ביצועיי המערכת הינה ייחודית לעבודה זו‪.‬‬ ‫‪102‬‬ ‫‪ - 3.1‬מודל שגיאות של כלי רכב עם זוויות החלקה במערכת קואורדינאטות קרטזית‬ ‫קבועה במרחב‬ ‫בסעיף זה מוצג פיתוח מודל שגיאות ביחס למסלול רצוי של כלי רכב בעל ארבעה‬ ‫גלגלים עם זוויות החלקה‪ .‬פיתוח מודל השגיאות מבוסס על מודל קינמאטי מסוג‬ ‫‪) Bicycle‬של כלי רכב בעל ארבעה גלגלים( הנתון באיור ‪ 3.1.1‬ומתואר ע"י משוואה‬ ‫)‪ (3.1.1‬אשר מאפשרת גלגול עם החלקה‪.‬‬ ‫במודל )‪ α1 , α 2 (3.1.1‬הן זוויות החלקה )של הגלגל הקדמי והאחורי בהתאמה(‬ ‫והמשתנים ‪ θ , y , x‬מתארים את מיקום וכיוון כלי הרכב במערכת קואורדינאטות‬ ‫קרטזית קבועה במרחב‪.‬‬ ‫‪φ‬‬ ‫‪α2‬‬ ‫‪v‬‬ ‫) ‪vtan ( φ − α 2‬‬ ‫‪α1‬‬ ‫‪θ‬‬ ‫‪vtan α1‬‬ ‫‪v‬‬ ‫‪y‬‬ ‫‪L‬‬ ‫‪x‬‬ ‫איור ‪ – 3.1.1‬מודל מסוג ‪ Bicycle‬הכולל זוויות החלקה‬ ‫מתוך איור ‪ 3.1.1‬ברור כי‪:‬‬ ‫) ‪cos ( θ + α1‬‬ ‫‪v = ( cos θ − tan α1 sin θ ) v‬‬ ‫) ‪cos ( α1‬‬ ‫)‪(3.1.1‬‬ ‫) ‪sin ( θ + α1‬‬ ‫‪v = ( sin θ + tan α1 cos θ ) v‬‬ ‫) ‪cos ( α1‬‬ ‫= &‪x‬‬ ‫= &‪y‬‬ ‫‪ tan ( φ − α 2 ) − tan α1 ‬‬ ‫‪θ& = ‬‬ ‫‪v‬‬ ‫‪L‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪&φ = w‬‬ ‫הבקר המוצג בהמשך משמש לצורך נהיגה אוטונומית של כלי רכב בעל ארבעה‬ ‫גלגלים לאורך מסלול רצוי הניתן לתיאור ע"י פונקציה ) ‪ y d = f (x‬רציפה וגזירה‬ ‫לפחות שלוש פעמים‪.‬‬ ‫‪103‬‬ ‫נגדיר את שגיאות כלי הרכב ביחס למסלול הרצוי באופן זהה להגדרה ששימשה‬ .(2.2.27) ‫בסיס לפיתוח הבקר‬ e1 = f ( x ) − y e2 = f ′ ( x ) cos θ − sin θ f ′ ( x ) sin θ tan φ cos θ tan φ − e3 = f ′′ ( x ) cos 2 θ − L L (3.1.2) ‫ שגיאה בכיוון הגלגלים הקדמיים של‬e 3 -‫ שגיאת כיוון ו‬e 2 ,‫ שגיאת מקום‬e1 ‫כאשר‬ :‫( מתקבל‬3.1.1) ‫ ממודל כלי הרכב‬φ& -‫ ו‬θ& , x& ‫( והצבה של‬3.1.2) ‫ מגזירה של‬.‫כלי הרכב‬ e&1 = ( f ′ ( x ) cos θ − sin θ ) v − ( f ′ ( x ) sin θ + cos θ ) tan α1v = e2 v + g1 ( θ, f ′, α1 ) v  f ′ ( x ) sin θ tan φ cos θ tan φ  − e& 2 =  f ′′ ( x ) cos 2 θ − v L L   f ′ ( x ) sin θ + cos θ + ( tan φ − tan ( φ − α2 ) ) v L  f ′ ( x ) sin θ + cos θ  + − f ′′ ( x ) cos θ sin θ  tan α1v L   ′ ′′ = e3v + g 2 ( θ, φ, f , f , α1 , α 2 ) v  3f ′′ ( x ) sin θ cos θ tan φ f ′ ( x ) cos θ tan 2 φ sin θ tan 2 φ  e& 3 =  f ′′′ ( x ) cos3 θ − − +  v 2 2  L L L   (3.1.3)  f ′ ( x ) sin θ + cos θ  &  f ′′ ( x ) sin 2 θ tan φ  2 ′′′ cos sin f x − φ − θ θ −   tan α1v ( )   L L cos 2 φ      2f ′′ ( x ) cos θ sin θ f ′ ( x ) cos θ tan φ sin θ tan φ  + + −  tan α1v L L2 L2    2f ′′ ( x ) cos θ sin θ f ′ ( x ) cos θ tan φ sin θ tan φ  + + −  ( tan φ − tan ( φ − α 2 ) ) v L L2 L2    3f ′′ ( x ) cos θ sin θ tan φ f ′ ( x ) cos θ tan 2 φ sin θ tan 2 φ  =  f ′′′ ( x ) cos3 θ − − +  v  L L2 L2    f ′ ( x ) sin θ + cos θ  −  w + g 3 ( θ, φ, f ′, f ′′, f ′′′, α1 , α 2 ) v 2 L cos φ   (‫( )אשר תוכנן על בסיס הנחה של גלגול ללא החלקה‬2.2.27) ‫כמו במקרה של הבקר‬ :‫גם כאן נבחר אות בקרה מהצורה‬ 104  f ′′ ( x ) cos θ sin θ tan φ f ′ ( x ) cos θ tan 2 φ − w =  f ′′′ ( x ) cos3 θ − 3 L L2   sin θ tan φ  L cos 2 φ + − v u   L2  f ′ ( x ) sin θ + cos θ  2 (3.1.4) ‫ יתוכנן בהמשך ע"פ קריטריון המבטיח מזעור ההשפעה של ההחלקות על‬u ‫כאשר‬ .‫השגיאות של כלי הרכב ביחס למסלול הרצוי‬ :‫( מתקבל מודל לא ליניארי מהצורה הבא‬1.1.4) -‫( ו‬1.1.3) ‫מתוך‬  g1 ( θ, f ′, α1 )  e&1  e 2 v    e&  =  e v  +  g θ, φ, f ′, f ′′, α , α 1 2) v  2  3   2 (  e& 3   u   g 3 ( θ, φ, f ′, f ′′, f ′′′, α1 , α 2 )  (3.1.5) .(3.1.3) -‫ מוגדרים ב‬g3 (⋅) -‫ ו‬g 2 (⋅) , g1 (⋅) ‫כאשר‬ :‫ ע"י שימוש בזהויות הבאות‬φr -‫ ו‬θr ‫נגדיר את הזוויות‬ f ′( x ) = sin θr cos θr f ′′ ( x ) = tan φr L cos3 θr (3.1.6) ‫ הם כיוון כלי הרכב וכיוון הגלגלים הקדמיים הדרושים לצורך נהיגה‬φr -‫ ו‬θr ‫כאשר‬ .‫של כלי הרכב על המסלול הרצוי‬ ‫( סביב המסלול הרצוי וסביב הנקודה‬3.1.5) ‫קירוב ליניארי )ליניאריזציה( של המודל‬ :‫ יהיה נתון ע"י‬,‫ כמקובל‬α1 = α 2 = 0  g11 ( θr )  0 0 1 0  0       e& =  0 0 1  ve +  0  u +  g 21 ( θr , φr ) g 22 ( θr , φr )  v  g31 ( θr , φr , f ′′′ ) g32 ( θr , φr )   0 0 0  1  144444244444 { 14243 3 B1 ( t ) = B1 ( θr , φr , f ′′′) B2 A  α1  α  { 2 (3.1.7) dα ( t ) :‫כאשר‬ g11 ( θr ) = − 1 cos θr g 22 ( θr , φr ) = , 1 L cos θr cos 2 φr g 21 ( θr , φr ) = , g31 ( θr , φr , f ′′′ ) = −f ′′′ ( x ) cos 2 θr sin θr + cos θr − tan φr sin θr L cos 2 θr g32 ( θr , φr ) = 2 tan φr sin θr L cos 2 φr cos 2 θr 2 sin 2 θr tan 2 φr + 2 tan φr cos θr sin θr L2 cos3 θr 105 (3.1.8) ‫הצד הימני של )‪ (3.1.7‬כולל חלק אחד ) ‪ ( Ave + Bu‬אשר זהה למודל שרשרת מסדר‬ ‫שלישי כפי שהוצג בפרק ‪) 2‬עבור ) ‪ ,( v ( t ) = v ( t‬וחלק נוסף ) ) ‪( B1 ( θr , φr , f ′′′ ) vd α ( t‬‬ ‫אשר ייקרא בעבודה זו "הפרעה"‪.‬‬ ‫המטריצה ) ‪ B1 ( t ) = B1 ( θr , φr , f ′′′‬תלויה בתכונות הגיאומטריות של המסלול הרצוי‪.‬‬ ‫מתוך )‪ (3.1.8‬ברור כי ניתן לתכנן מסלול רצוי כך שכל האיברים במטריצה זו הינם‬ ‫חסומים‪ .‬במקרה כזה קיימת מטריצה קבועה ‪ B1‬כך ש‪:‬‬ ‫)‪(3.1.8‬‬ ‫‪B1T ( t ) B1 ( t ) < B1T B1 , ∀t‬‬ ‫‪ – 3.2‬פיתוח בקר המבוסס על מודל קינמטי של כלי רכב עם זוויות החלקה‬ ‫העיקרון מאחורי פיתוח בקר זה מבוסס על בקרת ∞ ‪ , H‬דהיינו מזעור ההשפעה של‬ ‫ההפרעה )זוויות החלקה במקרה זה( על הפלט של המערכת )שגיאות ביחס למסלול‬ ‫הרצוי(‪ .‬הקריטריון המקובל בבקרת ∞ ‪ H‬מבוסס על שימוש בנורמת ‪ , L 2‬מכיוון‬ ‫שבהמשך נרצה להגדיר בעיית בקרה בזמן אינסופי ומכיוון שההפרעה ) ‪ d α ( t‬ב‪-‬‬ ‫)‪ (3.1.7‬אינה בעלת נורמת ‪ , L 2‬לא ניתן לעשות כאן שימוש בקריטריון המקובל‬ ‫בבקרת ∞ ‪ , H‬ולכן נשתמש כאן בקריטריון המבוסס על מזעור נורמת ∞‪. L‬‬ ‫נתונה מערכת ליניארית )קבועה בזמן(‪:‬‬ ‫)‪(3.2.1‬‬ ‫‪e ( e) ∈ ' n , dα ( e) ∈ ' m‬‬ ‫‪j‬‬ ‫‪e& = Ae + Bd α‬‬ ‫‪,‬‬ ‫' ∈ )‪z (e‬‬ ‫‪,‬‬ ‫‪z = Ce‬‬ ‫כאשר ) ‪ e ( t‬הינו ווקטור משתני המצב של המערכת‪ .‬אם המערכת )‪ (3.2.1‬יציבה‪ ,‬אז‬ ‫עבור תנאי התחלה ‪ e ( 0 ) = 0‬היא מעתיקה כניסה חסומה ∞‪ d α ∈ L‬ליציאה חסומה‬ ‫∞‪ . z ∈ L‬נסמן את המערכת ב‪ (3.2.1) -‬ע"י‪:‬‬ ‫‪L o : L∞ [ 0, ∞ ) , R m  → L ∞ [ 0, ∞ ) , R j ‬‬ ‫ונגדיר את נורמה של המערכת באופן הבא‪:‬‬ ‫)‪(3.2.2‬‬ ‫∞‬ ‫∞‬ ‫‪z‬‬ ‫‪dα‬‬ ‫כאשר ∞ ‪ z‬מוגדר ע"י‪:‬‬ ‫‪106‬‬ ‫‪= sup‬‬ ‫∞‪d α ∈L‬‬ ‫‪dα ≠ 0‬‬ ‫∞‬ ‫‪Lo‬‬ ‫})‪{ z (t‬‬ ‫)‪(3.2.3‬‬ ‫‪i‬‬ ‫‪' max sup‬‬ ‫) ∞ ‪i =1,.. p t∈[ 0‬‬ ‫∞‬ ‫‪z‬‬ ‫התיאוריה המוצגת במשפטים ‪ 1‬ו‪ 2 -‬קיימת ב‪ [Scherer & Weiland 1999] -‬ונקראת‬ ‫בקרת ‪ ,peak-to-peak‬משפט ‪ 3‬מהווה הרחבה של תיאוריה זו כך שתתאים למודל‬ ‫השגיאות )‪ (3.1.7‬שפותח בעבודה זו )הרחבה זו נדרשת מכיוון שמודל )‪ (3.1.7‬אינו‬ ‫קבוע בזמן(‪ .‬לצורך שלמות ובהירות התיאוריה ובגלל הבדלים מסוימים בניסוח )בין‬ ‫עבודה זו ו‪ ([Scherer & Weiland 1999] -‬משפטים ‪ 1‬ו‪ 2 -‬מוצגים עם הוכחות מלאות‪.‬‬ ‫משפט ‪ :1‬יהיה נתון ‪ , γ > 0‬אם קיימת מטריצה ‪ P > 0‬וסקלר ‪ λ > 0‬כך שמתקיימים‬ ‫אי‪ -‬השוויוניים )‪ (LMI's‬הבאים‪:‬‬ ‫‪ λP CT ‬‬ ‫‪‬‬ ‫‪>0‬‬ ‫‪ C γI j ‬‬ ‫)‪(3.2.4‬‬ ‫‪ A T P + PA + λnP PB ‬‬ ‫‪‬‬ ‫‪<0‬‬ ‫‪−γI m ‬‬ ‫‪BT P‬‬ ‫‪‬‬ ‫‪,‬‬ ‫כאשר ‪ n‬הוא המימד של המערכת‪.‬‬ ‫אזי נורמת המערכת מקיימת ‪< γ‬‬ ‫∞‬ ‫‪ Lo‬וכן המערכת יציבה אסימפטוטית פנימית‪.‬‬ ‫הוכחה‪:‬‬ ‫מתוך ה‪ LMI -‬השמאלי ב‪:(3.2.4) -‬‬ ‫‪d α T   A T P + PA + λnP PB   e ‬‬ ‫‪‬‬ ‫‪  < 0‬‬ ‫‪−γI m   d α ‬‬ ‫‪BT P‬‬ ‫‪‬‬ ‫)‪(3.2.5‬‬ ‫‪ T‬‬ ‫‪e‬‬ ‫)‬ ‫⇒‬ ‫= ‪x T A T + d α T BT Pe + eT P ( Ae + Bd α ) + λeT Pe − γd α T d α‬‬ ‫)‬ ‫(‬ ‫‪d T‬‬ ‫‪e Pe + λneT Pe − γd α T d α < 0‬‬ ‫‪dt‬‬ ‫(‬ ‫= ‪e& T Pe + eT Pe& + λneT Pe − γd α T d α‬‬ ‫מכאן נובע שהמערכת יציבה אסימפטוטית פנימית‪.‬‬ ‫נגדיר‪:‬‬ ‫)‪(3.2.6‬‬ ‫) ‪e ( t ) ' eT ( t ) Pe ( t‬‬ ‫מתוך )‪ (3.2.5‬ו‪:(3.2.6) -‬‬ ‫)‪(3.2.7‬‬ ‫‪T‬‬ ‫‪e& ( t ) + λne ( t ) − γd α ( t ) d α ( t ) < 0‬‬ ‫ע"י שימוש ב‪ [Khalil 1996]) Gronwall’s Inequality -‬עמוד ‪ (63‬ניתן להראות כי אי‬ ‫השוויון )‪ (3.2.7‬גורר‪:‬‬ ‫‪107‬‬ t e ( t ) < exp ( −λnt ) e ( 0 ) + ∫ exp ( −λn ( t − τ ) ) γd α ( τ ) d α ( τ ) dτ T 0 t  < exp ( −λnt ) e ( 0 ) +  ∫ exp ( −λn ( t − τ ) ) γdτ  n d α   0  (3.2.8) 2 ∞ :‫ מתקיים‬e ( 0 ) = 0 ‫עבור‬ e (0) = 0 t  e ( t ) <  ∫ exp ( −λn ( t − τ ) ) γdτ  n d α   0  2 ∞ = λ −1γ (1 − exp ( −λnt ) ) d α eT ( t ) Pe ( t ) < λ −1γ (1 − exp ( −λnt ) ) d α ⇒ 2 ∞ (3.2.9) 2 ∞ [Boyd et al. 1994] Schur complement -‫( וע"י שימוש ב‬3.2.4) -‫ הימני ב‬LMI -‫מתוך ה‬ :‫מתקבל‬  λP CT  −1 T   > 0 ⇒ λP − γ C C > 0  C γI j  ( ) ⇒ eT λP − γ −1CT C e > 0 (3.2.10) λeT Pe − γ −1eT CT Ce > 0 λeT Pe − γ −1z T z > 0 ⇒ z T ( t ) z ( t ) < γλeT ( t ) Pe ( t ) . e ( t ) -‫כאשר בשורה השנייה כפלנו את אי השוויון משני הצדדים ב‬ :‫ הראשון( מתקבל‬LMI -‫( )מה‬3.2.9) -‫בצרוף התוצאה הנתונה ב‬ z 2 ∞ ≤ z T ( t ) z ( t ) < γ 2 (1 − exp ( −λnt ) ) d α 2 ∞ ≤ γ 2 dα 2 ∞ , ∀t (3.2.10) ⇒ Lo ∞ <γ ' .‫ הראשון במשפט מהווה גם תנאי מספיק ליציבות של המערכת‬LMI -‫ קיום ה‬:‫הערה‬ ,Schur complement -‫טענה זו מבוססת על שימוש ב‬  A T P + PA + λnP PB   <0 ⇒ BT P −γIm   A T P + PA + λnP + PBγ −1BT P < 0 (3.2.11) A T P + PA < −λnP − PBγ −1BT P < 0 .‫ועל תורת היציבות של ליאפונוב‬ 108 ‫נדון עכשיו במערכת בעלת כניסה ‪ , u ∈ R‬הפרעה ‪ , d α ∈ ' m‬ופלט מבוקר ‪,z‬‬ ‫המוגדרים באופן הבא‪:‬‬ ‫‪e (0) = 0‬‬ ‫)‪(3.2.12‬‬ ‫‪z = Ce + Du ,‬‬ ‫‪,‬‬ ‫‪e& = Ae + B2 u + B1d α‬‬ ‫משוואות החוג הסגור של מערכת )‪ (3.2.12‬עם בקר משוב מצב ) ‪ u ( t ) = Ke ( t‬הן‪:‬‬ ‫)‪(3.2.13‬‬ ‫‪z = ( C + DK ) e = Ce‬‬ ‫‪,‬‬ ‫‪e& = ( A + B2K ) e + B1d α = Ae + B1d α‬‬ ‫מתקבלת מערכת בעלת כניסה ) ‪ d α ( t‬ויציאה ) ‪ , z ( t‬כמוראה באיור ‪:3.2.1‬‬ ‫)‪z(t‬‬ ‫‪e& = Ae + B2 u + B1d α‬‬ ‫) ‪e( t‬‬ ‫‪z = Ce + Du‬‬ ‫) ‪dα ( t‬‬ ‫‪u ( t ) = Ke‬‬ ‫‪K‬‬ ‫איור ‪ – 3.2.1‬מבנה מערכת עם בקר משוב מצב ) ‪ u ( t ) = Ke ( t‬והפרעה ) ‪d α ( t‬‬ ‫נסמן את האופרטור המעתיק את ) ‪ d ( t‬ליציאה המבוקרת ) ‪ z ( t‬ע"י‪:‬‬ ‫‪Lc1 : L∞ [ 0, ∞ ) , R m  → L∞ [ 0, ∞ ) , R j ‬‬ ‫אנו מעוניינים בבקר ) ‪ , u ( t ) = Ke ( t‬כך ש‪< γ , ( γ > 0 ) :‬‬ ‫∞‬ ‫‪Lc1‬‬ ‫משפט ‪ :2‬יהיה נתון ‪ , γ > 0‬הנח כי קיימים ‪ 0 < λ ∈ ' 1 , 0 < Q ∈ ' n×n‬ו‪ Y ∈ ' j×n -‬כך‬ ‫שמתקיימים אי השוויוניים הבאים‪:‬‬ ‫‪QA T + AQ + Y T B2 T + B2Y + λnQ B1 ‬‬ ‫‪‬‬ ‫‪<0‬‬ ‫‪B1T‬‬ ‫‪−γIm ‬‬ ‫‪‬‬ ‫)‪(3.2.14‬‬ ‫‪ λQ‬‬ ‫‪QCT + Y T DT ‬‬ ‫‪‬‬ ‫‪>0‬‬ ‫‪γI j‬‬ ‫‪CQ + DY‬‬ ‫‪‬‬ ‫אז הבקר ‪ K = YQ−1‬מבטיח ‪ Lc1 ∞ < γ‬והמערכת יציבה אסימפטוטית פנימית‪.‬‬ ‫הוכחה‪:‬‬ ‫מתוך משפט )‪ ,(1‬ברור כי נורמת המערכת )בחוג סגור( קטנה מ‪ γ -‬אם המערכת‬ ‫בחוג סגור יציבה וקיימים מטריצה ‪ P > 0‬וסקלר ‪ λ > 0‬כך שמתקיימים אי‪-‬‬ ‫השוויוניים הבאים‪:‬‬ ‫‪109‬‬ ‫)‪(3.2.15‬‬ ‫‪ A T P + PA + λnP PB1   A T P + K T B2 T P + PA + PB2K + λnP PB1 ‬‬ ‫‪‬‬ ‫‪=‬‬ ‫‪<0‬‬ ‫‪B1T P‬‬ ‫‪B1T P‬‬ ‫‪−γI m  ‬‬ ‫‪−γI m ‬‬ ‫‪‬‬ ‫‪λP CT   λP‬‬ ‫‪CT + K T D T ‬‬ ‫‪‬‬ ‫‪=‬‬ ‫‪>0‬‬ ‫‪γI j‬‬ ‫‪ C γI j   C + DK‬‬ ‫‪‬‬ ‫)אי שוויוניים אלה אינם ליניאריים ב‪ P -‬וב‪ K -‬בגלל הביטוי ‪ , PB2 K‬לכן אינם‬ ‫ניתנים לפתרון ע"י שיטות נומריות המתאימות ל‪(LMIs -‬‬ ‫מתוך הכפלה של אי השוויוניים האחרונים משני הצדדים במטריצה }‪diag {Q, I‬‬ ‫כאשר ‪ Q ' P −1‬והצבה ‪ Y = KQ‬מתקבל‪:‬‬ ‫)‪(3.2.16‬‬ ‫‪QA T + Y T B2 T + AQ + B2Y + λnQ B1 ‬‬ ‫‪‬‬ ‫‪<0‬‬ ‫‪B1T‬‬ ‫‪−γI m ‬‬ ‫‪‬‬ ‫‪ λQ‬‬ ‫‪QCT + Y T DT ‬‬ ‫‪‬‬ ‫‪>0‬‬ ‫‪γI j‬‬ ‫‪CQ + DY‬‬ ‫‪‬‬ ‫שהם אי השוויוניים )הליניאריים( הנתונים במשפט‪.‬‬ ‫'‬ ‫שימוש בתיאוריה המוצגת למעלה עבור מודלים קינמאטים של כלי רכב הכוללים‬ ‫זוויות החלקה )לדוגמה )‪ ((3.1.7‬מצריך הרחבה של התיאוריה למערכות תלויות בזמן‬ ‫מהצורה הבאה‪:‬‬ ‫)‪(3.2.17‬‬ ‫‪e ( 0) = 0‬‬ ‫‪e& ( t ) = Av ( t ) e ( t ) + B2 u ( t ) + B1 ( t ) v ( t ) d α ( t ) ,‬‬ ‫) ‪z ( t ) = Ce ( t ) + Du ( t‬‬ ‫כאשר ∞ < ‪ 0 < η1 < v ( t ) ∈ ' < η2‬ו‪ η2 , η1 ) B1T ( t ) B1 ( t ) < B1T B1 -‬ו‪ B1 -‬קבועים(‪.‬‬ ‫נסמן את המערכת )‪ (3.2.17‬ע"י‪:‬‬ ‫‪Lc2 : L∞ [ 0, ∞ ) , R m  → L∞ [ 0, ∞ ) , R j ‬‬ ‫במקרה זה מוצע שימוש בבקר משוב מצב מהצורה ) ‪ u ( t ) = Ke ( t ) v ( t‬ואנו‬ ‫מעוניינים לתכנן את קבועי הבקר ‪ , K‬כך ש‪< γ , ( γ > 0 ) :‬‬ ‫∞‬ ‫‪. Lc2‬‬ ‫משפט ‪ :3‬יהיה נתון ‪ , γ > 0‬הנח כי קיימים ‪ 0 < λ ∈ ' 1 , 0 < Q ∈ ' n×n‬ו‪ Y ∈ ' j×n -‬כך‬ ‫שמתקיימים אי השוויוניים הבאים‪:‬‬ ‫‪110‬‬ ‫(‬ ‫)‬ ‫‪ QA T + AQ + Y T B2T + B2 Y η1 + λnQ B1η2 ‬‬ ‫‪‬‬ ‫‪<0‬‬ ‫‪T‬‬ ‫‪‬‬ ‫‪B1 η2‬‬ ‫‪−γIm ‬‬ ‫‪‬‬ ‫)‪(3.2.18‬‬ ‫‪ λQ‬‬ ‫‪QCT + Y T DT ‬‬ ‫‪‬‬ ‫‪>0‬‬ ‫‪γI j‬‬ ‫‪CQ + DY‬‬ ‫‪‬‬ ‫אזי הבקר ‪ K = YQ −1‬מבטיח ‪< γ‬‬ ‫∞‬ ‫‪ Lc2‬והמערכת יציבה אסימפטוטית פנימית‪.‬‬ ‫הוכחה‪:‬‬ ‫ע"י שימוש בהוכחה דומה לזו של משפט )‪ (1‬ניתן להראות כי נורמת המערכת )בחוג‬ ‫סגור( קטנה מ‪ γ -‬אם קיימים מטריצה ‪ P > 0‬וסקלר ‪ λ > 0‬כך שמתקיימים אי‪-‬‬ ‫השוויוניים הבאים‪:‬‬ ‫)‪(3.2.19‬‬ ‫‪ ( A + B K )T P + P ( A + B K )  v ( t ) + λnP PB ( t ) v ( t ) ‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪‬‬ ‫‪<0‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪T‬‬ ‫) ‪B1 ( t ) Pv ( t‬‬ ‫‪−γIm ‬‬ ‫‪‬‬ ‫‪T‬‬ ‫‪ λP‬‬ ‫‪C +K D‬‬ ‫‪‬‬ ‫‪>0‬‬ ‫‪γI j‬‬ ‫‪C + DK‬‬ ‫‪‬‬ ‫‪T‬‬ ‫‪T‬‬ ‫שימוש ב‪) Schur complement -‬עבור אי השוויון הראשון ב‪:((3.2.19) -‬‬ ‫)‪(3.2.20‬‬ ‫‪ ( A + B K )T P + P ( A + B K )  v ( t ) + λnP PB ( t ) v ( t ) ‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪‬‬ ‫‪<0‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪T‬‬ ‫) ‪B1 ( t ) Pv ( t‬‬ ‫‪−γIm ‬‬ ‫‪‬‬ ‫⇒‬ ‫‪( A + B K )T P + P ( A + B K )  v ( t ) + λP + PB ( t ) γ −1B T ( t ) Pv 2 ( t ) < 0‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪1‬‬ ‫‪144444‬‬ ‫‪1444‬‬ ‫‪424444‬‬ ‫‪3‬‬ ‫‪2444443‬‬ ‫‪>0‬‬ ‫‪<0‬‬ ‫ברור כי קיום אי השוויון הבא מבטיח גם קיום של אי שוויון )‪:(3.3.20‬‬ ‫)‪(3.2.21‬‬ ‫‪( A + B K )T P + P ( A + B K )  η + λP + PB γ −1B T Pη2 < 0‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪1‬‬ ‫‪1 4244‬‬ ‫‪1 3‬‬ ‫‪2‬‬ ‫‪14‬‬ ‫‪144444‬‬ ‫‪2444443‬‬ ‫‪>0‬‬ ‫‪<0‬‬ ‫שימוש נוסף ב‪:Schur complement -‬‬ ‫)‪(3.2.22‬‬ ‫‪ ( A + B K )T P + P ( A + B K )  η + λnP PB η ‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪1 2‬‬ ‫‪‬‬ ‫‪ 1‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪B1T Pη2‬‬ ‫‪−γ −1I n ‬‬ ‫‪‬‬ ‫מתוך הכפלה של אי השוויוניים )‪ (3.2.22‬ואי השוויון השני ב‪ (3.2.19) -‬משני הצדדים‬ ‫במטריצה }‪ diag {Q, I‬כאשר ‪ Q ' P −1‬והצבה ‪ , Y = KQ‬מתקבלים‪:‬‬ ‫‪111‬‬ ‫(‬ ‫)‬ ‫‪ QA T + AQ + Y T B2T + B2 Y η1 + λnQ B1η2 ‬‬ ‫‪‬‬ ‫‪<0‬‬ ‫‪T‬‬ ‫‪‬‬ ‫‪−γIm ‬‬ ‫‪B1 η2‬‬ ‫‪‬‬ ‫)‪(3.2.23‬‬ ‫‪ λQ‬‬ ‫‪QCT + Y T DT ‬‬ ‫‪‬‬ ‫‪>0‬‬ ‫‪γI j‬‬ ‫‪CQ + DY‬‬ ‫‪‬‬ ‫שהם אי השוויוניים הנתונים במשפט‪.‬‬ ‫'‬ ‫הערות‪:‬‬ ‫‪ .1‬משפטים ‪ 2 ,1‬ו‪ 3 -‬אינם מבטיחים ‪ γ‬מינימאלית‪ ,‬ניתן למצוא ‪ γ‬קרובה ל‪γ -‬‬ ‫מינימאלית ע"י תהליך איטרטיבי של הקטנת ‪ γ‬כל עוד קיים פתרון למערכת ה‪-‬‬ ‫‪) LMIs‬תהליך כזה שימש בדוגמה הבאה(‪.‬‬ ‫‪ .2‬כאמור‪ ,‬כל התוצאות הללו תקפות עבור מערכת ליניארית ולכן גם עבור הקירוב‬ ‫הליניארי של מודל כלי הרכב‪ .‬נשאלת השאלה באיזו מידה תקפות תוצאות אלו‬ ‫עבור מודל כלי הרכב השלם )לא ליניארי(‪ .‬בהקשר זה ניתן להראות )בדיוק כפי‬ ‫שהוכח ב‪ ([van der Schaft 2000] -‬כי אם הקירוב הליניארי מקיים ‪< γ‬‬ ‫∞‬ ‫‪ Lc2‬אזי‬ ‫קיימת סביבה } ‪ U r ( 0 ) = {e : e ∞ < r‬שם‪:‬‬ ‫‪ .1‬המערכת יציבה אסימפטוטית פנימית‪.‬‬ ‫‪ .2‬עבור כל הפרעה ) ‪ d α ( t‬אשר אינה גורמת למערכת לעזוב את הסביבה ) ‪, U r ( 0‬‬ ‫מתקיים ‪< γ‬‬ ‫∞‬ ‫‪. Lc2‬‬ ‫תוצאות נומריות )כגון הדוגמא הבאה( מראות כי סביבה זו גדולה למדי )במובן‬ ‫המאפשר מימוש יעיל של התיאוריה בכלי רכב(‬ ‫דוגמא‬ ‫בדוגמה זו נדרשת נהיגה אוטונומית של כלי רכב בעל ארבעה גלגלים לאורך מסלול‬ ‫הנתון ע"י‪:‬‬ ‫)‪(3.2.24‬‬ ‫‪y ( x ) = 2 sin ( 0.25x ) + 0.25x + 1‬‬ ‫כאשר ‪ x‬משתנה מ‪ 0 -‬עד ‪ 40‬מטר ומהירות כלי הרכב היא ]‪. v = 5 [ m sec‬‬ ‫מתוך נתונים אלה ניתן לחשב את החסמים הבאים‪:‬‬ ‫)‪(3.2.25‬‬ ‫‪η1 = η2 = 5‬‬ ‫‪,‬‬ ‫‪112‬‬ ‫‪1 0 0 ‬‬ ‫‪B1 = 1.35 0 1 0 ‬‬ ‫‪0 0 1 ‬‬ ‫נתונים נוספים בדוגמה‪:‬‬ ‫‪0 0‬‬ ‫‪0 ‬‬ ‫‪‬‬ ‫‪0 ‬‬ ‫‪1 0‬‬ ‫‪, D =   , λ = 1 , γ = 13‬‬ ‫‪0 ‬‬ ‫‪0 3‬‬ ‫‪‬‬ ‫‪ ‬‬ ‫‪0 0‬‬ ‫‪1 ‬‬ ‫)‪(3.2.26‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪C=‬‬ ‫‪0‬‬ ‫‪‬‬ ‫‪0‬‬ ‫מפתרון ה‪ LMI's -‬ב‪ Matlab LMI toolbox -‬מתקבל‪:‬‬ ‫]‪K = [ −2.8 −8.3 −7.16‬‬ ‫)‪(3.2.27‬‬ ‫תוצאות סימולציה של בקר זה המופעל עם מודל קינמטי ללא החלקות מוצגות‬ ‫בגרף ‪ ,3.1‬לצורך השוואה בהמשך כאשר הבקר מופעל עם מודל דינאמי‪ ,‬מוצגות גם‬ ‫תוצאות של בקר משוב מצב אחר )לא מבוסס ‪ (LMI‬בעל ביצועים דומים כאשר‬ ‫מופעל עם מודל קינמטי ללא החלקות )ראה גרף ‪ ,(3.2‬הגברי בקר זה הם‪:‬‬ ‫]‪K = [ −1 −3 −2‬‬ ‫)‪(3.2.28‬‬ ‫כדי לבחון את ביצועי הבקר בוצעו סימולציות עם שני מודלים דינאמיים של כלי‬ ‫רכב בעלי ארבעה גלגלים אשר אינם מבוססים על הנחה של "גלגול ללא החלקה"‬ ‫)לצורך השוואה מוצגות גם תוצאות של בקר )‪.((3.2.28‬‬ ‫אחד המודלים הינו מודל דינאמי מסוג ‪ .Bicycle‬פיתוח מודל זה מבוסס על‬ ‫דיאגראמת הכוחות והמהירויות הנתונה באיור ‪.3.2.2‬‬ ‫‪y‬‬ ‫‪l2‬‬ ‫‪φ‬‬ ‫‪x‬‬ ‫‪α2‬‬ ‫&‪v y − l2θ‬‬ ‫‪vx‬‬ ‫&‪θ‬‬ ‫‪v‬‬ ‫‪β‬‬ ‫‪vx‬‬ ‫‪Fy2‬‬ ‫‪l1‬‬ ‫‪vy‬‬ ‫‪c.g.‬‬ ‫&‪v y + l1θ‬‬ ‫‪α1‬‬ ‫‪θ‬‬ ‫‪Fx1‬‬ ‫‪vx‬‬ ‫‪X‬‬ ‫‪Fx 2‬‬ ‫‪Fy1‬‬ ‫איור ‪ – 3.2.2‬דיאגראמת כוחות ומהירויות במודל דינאמי מסוג ‪.Bicycle‬‬ ‫משוואות התנועה של המודל באיור ‪ 3.3.2‬הן ]‪:[Wong 2001‬‬ ‫(‬ ‫)‬ ‫‪M ( v& y + v x θ& ) = Fy1 + Fx 2 sin φ + Fy2 cos φ‬‬ ‫‪M v& x − v yθ& = Fx1 + Fx 2 cos φ − Fy2 sin φ‬‬ ‫)‪(3.3.29‬‬ ‫‪Y‬‬ ‫&&‪Izz‬‬ ‫‪θ = −Fy1l1 + Fx 2l2 sin φ + Fy2l2 cos φ‬‬ ‫‪113‬‬ ‫כאשר ‪ M‬מסה‪ Izz ,‬מומנט אינרציה ביחס לציר ‪) z‬ניצב למישור ) ‪ ( ( x , y‬העובר דרך‬ ‫מרכז הכובד של כלי הרכב‪ v x , v y ,‬הן מהירויות של מרכז הכובד‪ Fx , Fy ,‬כוחות‬ ‫אורכיים וצידיים הפועלים על הגלגלים ו‪ l1 , l2 -‬קובעים את מיקום מרכז הכובד של‬ ‫כלי הרכב לאורך ציר ‪ .x‬במקרה של הנעה אחורית )המקרה הנדון בעבודה זו(‬ ‫‪ Fx2 = 0‬ו‪ Fx1 -‬הוא הכוח המועבר לגלגלים מהמנוע‪.‬‬ ‫מתוך הדיאגראמה ברור כי הקשר בין המהירויות ובין זוויות ההחלקה נתון ע"י‪:‬‬ ‫)‪(3.3.30‬‬ ‫‪ l1θ& − v y ‬‬ ‫‪⇒ α1 = atan ‬‬ ‫‪ v x ‬‬ ‫‪‬‬ ‫‪‬‬ ‫&‬ ‫‪ l2θ + v y ‬‬ ‫‪⇒ α 2 = φ − atan ‬‬ ‫‪ v x ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪l1θ& − v y‬‬ ‫‪vx‬‬ ‫‪l 2θ& + v y‬‬ ‫‪vx‬‬ ‫= ‪tan α1‬‬ ‫= ) ‪tan ( φ − α 2‬‬ ‫בנוסף‪ ,‬הקשר בין זוויות ההחלקה ובין הכוחות הצידיים הפועלים על הגלגלים‬ ‫מבוסס על המודל‪:‬‬ ‫)‪(3.3.31‬‬ ‫‪Fy1 = 2Cα1α1‬‬ ‫‪Fy2 = 2Cα 2α 2‬‬ ‫כאשר הקבועים ‪ (Cornering stiffness) Cα1 , Cα 2‬מאפיינים כל אחד גלגל יחיד )קדמי‬ ‫או אחורי(‪ ,‬וכל כוח ) ‪ Fy2‬או ‪ ( Fy1‬הינו תולדה של שני גלגלים השייכים לסרן משותף‪.‬‬ ‫הפרמטרים הפיזיקאליים במודל הדינאמי הם‪:‬‬ ‫)‪(3.3.32‬‬ ‫]‪M = 1700 [Kg], L = 2.7 [m], l1 = 1.58 [m], l2 = 1.12 [m‬‬ ‫]‪I zz = 3332 [kg ⋅ m 2 ], Cα1 = 100 [N/deg], Cα 2 = 110 [N/deg‬‬ ‫המודל הדינאמי השני )המשמש כאן לסימולציה( מתקבל מתוך תוכנת ‪CarSim‬‬ ‫המקובלת בתעשייה לתכנון ואנליזה של כלי רכב‪ .‬התוכנה מייצרת מודל ע"פ‬ ‫פרמטרים פיזיקאליים רצויים‪ ,‬ראה דוגמה של אחד מתפריטי התוכנה )עם‬ ‫הפרמטרים המשמשים את הסימולציה המוצגת בהמשך( באיור ‪ .3.2.3‬המודל מה‪-‬‬ ‫‪ CarSim‬ניתן להפעלה בתוכנת ‪ Simulink‬שם מבוצעת הסימולציה בחוג סגור עם‬ ‫הבקר‪ .‬מודל התוכנה הינו תלת ממדי ומבוסס על ‪ 9‬גופים קשיחים עם ‪ 10‬דרגות‬ ‫חופש‪ .‬היתרון בשימוש בתוכנה זו הוא ביצוע סימולציה עם מודל מורכב ואמין‬ ‫המתאים לתיאור כלי רכב ממשיים )לדוגמא כאן נעשה שימוש במודל של מכונית‬ ‫נוסעים( החיסרון הוא בכך שהמשוואות הדינאמיות אינן נתונות‪ .‬מסיבה זו מוצגות‬ ‫כאן גם סימולציות עם מודל דינאמי פשוט יותר מסוג ‪) Bicycle‬אשר נתון במשוואות‬ ‫‪.(3.2.29-3.2.32‬‬ ‫‪114‬‬ ‫איור ‪ – 3.2.3‬תפריט פרמטרים של מכונית נוסעים בתוכנה ‪.CarSim‬‬ ‫בגרפים ‪ 3.3-3.6‬מוצגות תוצאות שהתקבלו מהפעלת הבקר )‪) (3.2.27‬מבוסס ‪(LMI‬‬ ‫עם המודל הדינאמי של התוכנה ‪ CarSim‬ובגרפים ‪ 3.7-3.10‬מוצגות תוצאות‬ ‫שהתקבלו מהפעלת הבקר )‪) (3.2.28‬לא מבוסס ‪ (LMI‬עם אותו מודל )‪ .(CarSim‬ניתן‬ ‫לראות כי במקרה של הבקר שאינו מבוסס ‪ LMI‬כלי הרכב מבצע תנודות לא‬ ‫מרוסנות סביב המסלול הרצוי ונדרש אות בקרה גדול בהרבה )למרות שהגברי הבקר‬ ‫במקרה זה קטנים יותר(‪ .‬רושם דומה ניתן לקבל גם מהשוואה של גרפי השגיאות ‪3.5‬‬ ‫)מבוסס ‪ (LMI‬עם ‪) 3.9‬לא מבוסס ‪(LMI‬‬ ‫בגרפים ‪ 3.11-3.14‬מוצגות תוצאות שהתקבלו מהפעלת הבקרים עם מודל דינאמי‬ ‫מסוג ‪ .Bicycle‬גם במקרה זה הביצועים של הבקר )‪) (3.2.27‬מבוסס ‪ (LMI‬טובים‬ ‫מהביצועים של בקר )‪) (3.2.28‬לא מבוסס ‪.(LMI‬‬ ‫למרות שהבסיס התיאורטי של בקר )‪) (3.2.27‬מבוסס ‪ (LMI‬כפי שמוצג בפרק זה תקף‬ ‫בסביבה קרובה של המסלול הרצוי )מבוסס על לינאריזציה(‪ ,‬סימולציות רבות‬ ‫)לדוגמא אלה המוצגות( מראות כי התיאוריה המוצגת "שמרנית" וניתן לקבל‬ ‫ביצועים טובים גם כאשר כלי הרכב אינו קרוב מאוד למסלול הרצוי‪.‬‬ ‫‪115‬‬ Trajectory trajectory desired trajectory 20 15 y [m] 10 5 0 -5 0 5 10 15 20 x [m] 25 30 35 40 .((1.2) ‫ מופעל עם מודל קינמטי‬LMI ‫ – מסלול נסיעה )בקר מבוסס‬3.1 ‫גרף‬ Trajectory trajectory desired trajectory 20 15 y [m] 10 5 0 -5 0 5 10 15 20 x [m] 25 30 35 40 .((1.2) ‫ מופעל עם מודל קינמטי‬LMI ‫ – מסלול נסיעה )בקר לא מבוסס‬3.2 ‫גרף‬ 116 Trajectory trajectory desired trajectory 20 15 y [m] 10 5 0 -5 0 5 10 15 20 x [m] 25 30 35 40 .(CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – מסלול נסיעה )בקר מבוסס‬3.3 ‫גרף‬ Control input 8 6 4 w(t) 2 0 -2 -4 -6 -8 0 2 4 6 time [sec] 8 10 12 .(CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – אות בקרה )בקר מבוסס‬3.4 ‫גרף‬ 117 Tracking Errors 2 e1(t) 0 -2 -4 0 2 4 6 time [sec] 8 10 12 0 2 4 6 time [sec] 8 10 12 0 2 4 6 time [sec] 8 10 12 1 e2(t) 0.5 0 -0.5 0.4 e3(t) 0.2 0 -0.2 -0.4 .(CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – שגיאות )בקר מבוסס‬3.5 ‫גרף‬ Vehicle slip Angles 10 0 α 1 [deg] 5 -5 -10 0 2 4 6 time [sec] 8 10 12 0 2 4 6 time [sec] 8 10 12 20 0 α 2 [deg] 10 -10 -20 .(CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – זוויות החלקה )בקר מבוסס‬3.6 ‫גרף‬ 118 Trajectory trajectory desired trajectory 20 15 y [m] 10 5 0 -5 0 5 10 15 20 x [m] 25 30 35 40 .(CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – מסלול נסיעה )בקר לא מבוסס‬3.7 ‫גרף‬ Control input 8 6 4 w(t) 2 0 -2 -4 -6 -8 0 2 4 6 8 time [sec] 10 12 14 16 .(CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – אות בקרה )בקר לא מבוסס‬3.8 ‫גרף‬ 119 Tracking Errors 2 e1(t) 0 -2 -4 0 2 4 6 8 time [sec] 10 12 14 16 0 2 4 6 8 time [sec] 10 12 14 16 0 2 4 6 8 time [sec] 10 12 14 16 1 e2(t) 0.5 0 -0.5 1 e3(t) 0.5 0 -0.5 -1 (CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – שגיאות )בקר לא מבוסס‬3.9 ‫גרף‬ Vehicle slip Angles 10 0 α 1 [deg] 5 -5 -10 0 2 4 6 8 time [sec] 10 12 14 16 0 2 4 6 8 time [sec] 10 12 14 16 0 α 2 [deg] 50 -50 .(CarSim ‫ מופעל עם מודל דינאמי‬LMI ‫ – זוויות החלקה )בקר לא מבוסס‬3.10 ‫גרף‬ 120 Trajectory trajectory desired trajectory 20 15 y [m] 10 5 0 -5 0 5 10 15 20 x [m] 25 30 35 40 .(Bicycle ‫ מופעל עם מודל דינאמי‬LMI ‫ – מסלול נסיעה )בקר מבוסס‬3.11 ‫גרף‬ Control input 4 3 2 w(t) 1 0 -1 -2 -3 -4 0 2 4 6 time [sec] 8 10 12 .(Bicycle ‫ מופעל עם מודל דינאמי‬LMI ‫ – אות בקרה )בקר מבוסס‬3.12 ‫גרף‬ 121 Trajectory trajectory desired trajectory 20 15 y [m] 10 5 0 -5 0 5 10 15 20 x [m] 25 30 35 40 .(Bicycle ‫ מופעל עם מודל דינאמי‬LMI ‫ – מסלול נסיעה )בקר לא מבוסס‬3.13 ‫גרף‬ Control input 4 3 2 w(t) 1 0 -1 -2 -3 -4 0 2 4 6 8 10 12 14 time [sec] .(Bicycle ‫ מופעל עם מודל דינאמי‬LMI ‫ – אות בקרה )בקר לא מבוסס‬3.14 ‫גרף‬ 122 ‫‪ – 4‬מימוש ופתרון בעיות מעשיות‬ ‫אחד הקווים המנחים של עבודת מחקר זו הוא פיתוח אלגוריתמיים אשר יהיו‬ ‫ישימים ומתאימים למימוש בכלי רכב‪ .‬לצורך השגת מטרה זו ניתן דגש בעבודה על‬ ‫ניסוי ומימוש האלגוריתמים בתנאי שדה )‪ .(outdoor‬בנספח ‪ A.1.1‬מוצג כלי רכב‬ ‫חשמלי הכולל מחשב‪ ,‬מפעילים וחיישנים שונים ופותח במסגרת עבודת המחקר‪.‬‬ ‫תוצאות ניסויים שבהם מומשו אלגוריתמים מתוך עבודת המחקר במערכת הניסוי‬ ‫מוצגים בהמשך‪ .‬אחת הבעיות המורכבות הקשורות בהפעלת כלי רכב אוטונומיים‬ ‫היא מדידת מצב כלי הרכב במרחב )כלומר ביחס למערכת צירים אינרציאלית(‪.‬‬ ‫באופן כללי מצב כלי רכב במרחב מבוסס על ‪ 6‬דרגות חופש של גוף קשיח במרחב‬ ‫)כיוון הגלגלים הקדמיים נמדד ביחס למסגרת כלי הרכב(‪ .‬מכיוון שאנו דנים כאן‬ ‫במסלולים מישוריים בלבד מצב כלי הרכב במרחב מבוסס על שלושה משתנים‬ ‫) ‪ . ( x , y,θ‬מדידת משתני מצב אלה מבוססת על שימוש ב‪ DGPS -‬ומצפן מגנטי‪.‬‬ ‫המידע המתקבל ישירות מחיישנים אלה סובל מבעיות רעש ודיוק‪ ,‬לכן יכול לשמש‬ ‫להפעלת מערכות הבקרה רק אחרי שעבר עיבוד נוסף‪ .‬מכיוון שמודל כלי רכב בעל‬ ‫ארבעה גלגלים )‪ (1.2‬אינו ליניארי‪ ,‬השיטה המקובלת לסינון ושערוך בכלי רכב‬ ‫אוטונומיים היא שימוש במסנן קלמן מורחב )‪ ,(EKF‬מסנן כזה מומש במערכת‬ ‫הניסוי וסיפק תוצאות טובות‪ .‬בנוסף פותחה בעבודה זו גישה שונה המוצגת בסעיף‬ ‫‪ 4.1‬ומאפשרת שימוש במסנן קלמן רגיל )מסנן המיועד להפעלה במערכות בעלות‬ ‫מודל ליניארי(‪.‬‬ ‫משתנה המצב הרביעי של כלי הרכב ‪) φ‬כיוון הגלגלים הקדמיים ביחס למסגרת כלי‬ ‫הרכב( הינו פשוט יותר למדידה וחישן נפוץ וזול יחסית כגון אנקודר יכול לספק‬ ‫מדידה ברמת דיוק וחלקות טובים )לצורכי מימוש(‪.‬‬ ‫נושא נוסף הנדון בפרק זה הוא תכנון מסלול הייחוס‪ .‬בפרקים ‪ 2‬ו‪ 3 -‬של העבודה‬ ‫הוצגו בקרים המיועדים לנהיגה אוטונומית של כלי רכב בעל ‪ 4‬גלגלים לאורך מסלול‬ ‫רצוי הניתן לייצוג באופן אנליטי )ע" פונקציות מהצורה ) ‪ yr = f ( x‬או עקום פרמטרי‬ ‫) ‪ ( x r = f x ( s ) , y r = f y ( s‬עם דרישות מסוימות כגון רציפות וחסמים של הנגזרות‪.‬‬ ‫לעומת זאת הדרך המקובלת והנוחה לתכנון מסלול רצוי לכלי רכב אוטונומי הנע‬ ‫בתנאי שדה היא ע"י סדרה של נקודות דרך )‪ (way points‬אשר נקבעות בדרך כלל על‬ ‫סמך מדידות או מודל של השטח )מפה‪ ,‬תצ"א(‪ .‬כדי להתאים בין הגישות השונות‬ ‫ולאפשר מימוש מערכות הבקרה )פרקים ‪ 2‬ו‪ (3 -‬פותחו שני אלגוריתמים לתכנון‬ ‫מסלול העובר בין נקודות דרך רצויות‪ .‬סעיף ‪ 4.2‬עוסק בתכנון מסלול עבור בקרים‬ ‫‪123‬‬ ‫המיועדים להפעלה בגישה של מערכת קואורדינאטות קבועה במרחב וסעיף ‪4.3‬‬ ‫עוסק בתכנון מסלול עבור בקרים המיועדים להפעלה בגישה של מערכת‬ ‫קואורדינאטות מקומית‪ .‬שני האלגוריתמים )לתכנון מסלול( מתאימים להפעלה ע"י‬ ‫מחשב כלי הרכב בזמן אמת ושולבו במערכת הבקרה לפי גישה זו )כלומר במערכת‬ ‫המשולבת המותקנת בכלי הרכב אות הייחוס הוא סדרה של נקודות דרך(‪ .‬סעיף ‪4.2‬‬ ‫כולל גם הצגה של תוצאות ניסוי‪.‬‬ ‫‪ - 4.1‬מימוש מסנן ‪Kalman Filter‬‬ ‫שילוב מסנן קלמן רציף )‪ (Kalman-Bucy Filter‬בכלי הרכב נועד לענות על הצרכים‬ ‫הבאים‪:‬‬ ‫‪ .1‬סינון רעשים הנלווים לאותות הנמדדים‪ ,‬לדוגמה מדידת המקום המתקבלת‬ ‫מחיישן ה‪ GPS -‬מלווה ברעש אשר מקורו בדיוק המוגבל של החישן )כ‪0.5 -‬‬ ‫מטר( ובקצב הדגימה הנמוך יחסית )‪.(10 Hz‬‬ ‫‪ .2‬שיערוך משתני מצב אשר אינם נמדדים באופן ישיר‪ ,‬לדוגמא ברכב הניסוי‬ ‫מותקן מצפן מגנטי אולם עקב מגבלות רעש‪ ,‬דיוק והשפעות סביבה לא ניתן‬ ‫כרגע לעשות בו שימוש לצורך מדידת הכיוון של כלי הרכב‪ .‬כיוון כלי הרכב‬ ‫משוערך ע"י שימוש במסנן קלמן‪ .‬שיערוך זה מבוסס על מדידות המקום‬ ‫)‪ ,(GPS‬זווית הגלגלים הקדמיים ושימוש במודל המתמטי של כלי הרכב‪.‬‬ ‫הערה‪ :‬למרות המגבלות שהוזכרו בהקשר לשימוש במצפן המגנטי והעובדה שכרגע‬ ‫לא נעשה בו שימוש רציף למדידת הכיוון של כלי הרכב‪ ,‬הוא מביא תועלת רבה בכך‬ ‫שהוא מספק כיוון התחלתי )תנאי התחלה( למשערך‪.‬‬ ‫מודל כלי הרכב לצורך שיערוך נתון ב‪.(4.1.1) -‬‬ ‫‪π‬‬ ‫‪+ ε, ε > 0‬‬ ‫‪2‬‬ ‫)‪(4.1.1‬‬ ‫<‪φ‬‬ ‫‪ v cos θ ‬‬ ‫‪ x&  ‬‬ ‫‪‬‬ ‫‪ y&   v sin θ ‬‬ ‫‪χ& ( t ) =  &  =  tan φ  = f ( χ, v, w ) ,‬‬ ‫‪θ v‬‬ ‫‪‬‬ ‫‪&  L ‬‬ ‫‪φ‬‬ ‫‪   w ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ x   ν1 ‬‬ ‫‪z ( t ) = h ( χ ) + ν =  y  +  ν 2 ‬‬ ‫‪ φ   ν3 ‬‬ ‫כאשר ‪ z1 = x + ν1 , z2 = y + ν2‬מדידות ‪ GPS‬רועשות‪ z3 = φ + ν3 ,‬מדידה של כיוון‬ ‫הגלגלים הקדמיים‪ ν ,‬רעש מדידה‪) v ,‬מהירות קווית( ו‪) w -‬אות בקרה( )‪ v‬ו‪w -‬‬ ‫‪124‬‬ ‫נתונים(‪ .‬מודל זה מבוסס על הנחה של העדר רעש תהליך‪ .‬המודל )‪ (4.1.1‬אינו‬ ‫ליניארי‪ ,‬לכן הבחירה הטבעית והמקובלת לסינון ושיערוך במקרה זה מבוססת על‬ ‫שימוש במסנן קלמן מורחב ‪ .Extended Kalman-Bucy Filter‬מסנן זה הינו הרחבה של‬ ‫המסנן הרגיל )המיועד למערכות ליניאריות( ומבוסס על ליניאריזציה‪ ,‬במקרה זה‬ ‫למסנן אין בסיס מתמטי המבטיח התכנסות אולם בהרבה מקרים )כמו גם במקרה‬ ‫זה( הוא ניתן למימוש ביעילות רבה‪.‬‬ ‫מודל המסנן שמומש בכלי הרכב נתון ב‪) (4.1.2) -‬ראה ]‪.([Stengel 1994‬‬ ‫)‪(4.1.2‬‬ ‫‪χˆ& ( t ) = f ( χ, v, w ) + K ( t )  z ( t ) − h ( χˆ ( t ) )  , χˆ ( 0 ) = χˆ 0‬‬ ‫‪K ( t ) = P ( t ) H T R −1‬‬ ‫כאשר ‪ χ‬שיערוך של ‪ χ‬ו‪ P ( t ) > 0 -‬פתרון של משוואת ריקטי מהצורה‪:‬‬ ‫)‪(4.1.3‬‬ ‫‪T‬‬ ‫‪P& ( t ) = F ( t ) P ( t ) + P ( t ) F ( t ) − P ( t ) H T R −1HP ( t ) , P ( 0 ) = P0‬‬ ‫המטריצות ) ‪ F ( t‬ו‪ H -‬נתונות ע"י‪:‬‬ ‫‪‬‬ ‫‪0 ‬‬ ‫‪v ‬‬ ‫‪‬‬ ‫‪L cos 2 φ ‬‬ ‫‪0 ‬‬ ‫‪0‬‬ ‫)‪(4.1.4‬‬ ‫‪0 − v sin θ‬‬ ‫‪v cos θ‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪0 0 0‬‬ ‫‪1 0 0 ‬‬ ‫‪0 0 1 ‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪∂f ( χ, v, w ) ‬‬ ‫‪=‬‬ ‫= ) ‪F( t‬‬ ‫‪∂χ‬‬ ‫‪0‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪1‬‬ ‫‪∂h ( χ ( t ) ) ‬‬ ‫= )‪H(t‬‬ ‫‪= 0‬‬ ‫‪∂χ‬‬ ‫‪0‬‬ ‫ו‪ R -‬קווריאנס רעש המדידה‪.‬‬ ‫בניסוי שתוצאותיו נתונים בסעיף ‪ 4.2‬מומש מסנן ‪ EKF‬עם הפרמטרים הבאים‪:‬‬ ‫)‪(4.1.5‬‬ ‫‪‬‬ ‫‪z1 ( 0 ) ‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪10 0‬‬ ‫‪100 0 0 ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪z‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫(‬ ‫)‬ ‫‪2‬‬ ‫‪ , P =‬‬ ‫‪ , R =  0 100 0 ‬‬ ‫‪χˆ 0 = ‬‬ ‫‪0‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪θCOMPASS ( 0 ) ‬‬ ‫‪ 0 0 100 0 ‬‬ ‫‪ 0‬‬ ‫‪0 1 ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪z3 ( 0 ) ‬‬ ‫‪0 10 ‬‬ ‫‪0 0‬‬ ‫‪‬‬ ‫הערות‪:‬‬ ‫‪ zi ( 0 ) .1‬מסמן מדידה בזמן אפס לפי )‪ θCOMPASS ( 0 ) ,(4.1.1‬מסמן מדידה של המצפן‬ ‫בזמן אפס‪.‬‬ ‫‪ .2‬הערכים המספריים הנתונים עבור ‪ R‬ו‪ P0 -‬נמצאו כערכים המביאים לביצועים‬ ‫טובים של מערכת הבקרה עם המסנן‪ .‬ב‪ R -‬ניתן משקל גדול למשתני המצב ‪x , y‬‬ ‫‪125‬‬ ‫המתקבלים מתוך מדידות ‪ GPS‬רועשות וב‪ P0 -‬ניתן משקל גדול למשתנה המצב ‪θ‬‬ ‫בגלל אפשרות של שגיאה התחלתית גדולה במשתנה זה‪.‬‬ ‫החיסרון של המסנן המורחב ביחס למסנן הרגיל )המיועד למודל ליניארי( הוא העדר‬ ‫הבסיס המתמטי הקיים עבור המסנן הרגיל‪ .‬ניתן ע"י שימוש בטרנספורמציה פשוטה‬ ‫להפוך את המודל ב‪ (4.1.1) -‬למודל ליניארי תלוי בזמן המאפשר שימוש במסנן קלמן‬ ‫רגיל‪ .‬טרנספורמציה זו מבוססת על הנחה כי משתנה המצב ‪ φ‬נמדד "באיכות" טובה‬ ‫לכן לא מצריך סינון או שיערוך ולמעשה במודל החדש הוא אינו משתנה מצב אלה‬ ‫אות )תלוי בזמן( נתון‪.‬‬ ‫מלבד משתני המצב ‪ x‬ו‪ y -‬נגדיר שני משתני מצב נוספים ‪ c‬ו‪:s -‬‬ ‫⇒ ) ) ‪c ( t ) = cos ( θ ( t ) ) , s ( t ) = sin ( θ ( t‬‬ ‫) ‪tan ( φ‬‬ ‫‪c& = − ( sin θ ) θ& = −s‬‬ ‫‪v‬‬ ‫‪L‬‬ ‫) ‪tan ( φ‬‬ ‫‪s& = ( cos θ ) θ& = c‬‬ ‫‪v‬‬ ‫‪L‬‬ ‫)‪(4.1.6‬‬ ‫מתוך )‪ (4.1.6‬ו‪ (4.1.1) -‬מתקבל המודל הבא‪:‬‬ ‫‪π‬‬ ‫‪+ ε, ε > 0‬‬ ‫‪2‬‬ ‫<‪φ‬‬ ‫‪1‬‬ ‫‪0‬‬ ‫‪ x‬‬ ‫‪ x&   0 0‬‬ ‫‪  ‬‬ ‫‪ y&   0 0‬‬ ‫‪0‬‬ ‫‪1‬‬ ‫‪ v  y ,‬‬ ‫‪ =‬‬ ‫‪ c&   0 0‬‬ ‫‪− ( tan φ ) L   c ‬‬ ‫‪0‬‬ ‫‪  ‬‬ ‫‪  ‬‬ ‫‪0‬‬ ‫‪s&   0 0 ( tan φ ) L‬‬ ‫{‪‬‬ ‫‪s ‬‬ ‫{ ‪3‬‬ ‫‪14444442444444‬‬ ‫‪χ1‬‬ ‫) ‪A ( t ) = A ( φ, v‬‬ ‫‪χ& 1‬‬ ‫)‪(4.1.7‬‬ ‫‪1 0 0 0   x   ν1 ‬‬ ‫‪z=‬‬ ‫‪ +‬‬ ‫‪0 1 0 0   y   ν 2 ‬‬ ‫‪14‬‬ ‫‪4244‬‬ ‫‪3‬‬ ‫{‬ ‫‪c ‬‬ ‫‪ν‬‬ ‫‪C‬‬ ‫‪ ‬‬ ‫‪s‬‬ ‫{‪‬‬ ‫‪χ1‬‬ ‫במקרה של המודל ב‪ (4.1.7) -‬נעשה שימוש מסנן קלמן רציף )‪(Kalman-Bucy Filter‬‬ ‫]‪ [Stengel 1994‬מהצורה‪:‬‬ ‫‪χˆ& = A ( φ, v ) χˆ + K ( t )( z − Cχˆ ) , χˆ ( 0 ) = χ0‬‬ ‫‪K ( t ) = P ( t ) CT R −1‬‬ ‫)‪(4.1.8‬‬ ‫‪P& ( t ) = A ( φ, v ) P ( t ) + P ( t ) A T ( φ, v ) − P ( t ) CT R −1CP ( t ) , P ( 0 ) = P0‬‬ ‫בגרפים ‪ 4.1-4.6‬מוצגות תוצאות של סימולציה אשר בוצעה עם מודל של כלי רכב‬ ‫בעל ארבעה גלגלים‪ ,‬בקר )‪ (2.2.27‬ומשערך )‪ .(4.1.8‬תנאי ההתחלה של כלי הרכב‬ ‫בסימולציה הם ‪ , x ( 0 ) = 0 [ m ] , y ( 0 ) = 5, θ ( 0 ) = 0, φ ( 0 ) = 0‬המרחק בין ציר הגלגלים‬ ‫‪126‬‬ ‫הקדמי והאחורי ]‪, L = 1 [m‬המסלול הרצוי ‪ , y ( x ) = sin ( x ) + 0.5x + 1‬המהירות‬ ‫הקווית ]‪ v = 1 [ m / s‬ונתוני הבקר הם‪ . k = 4, a1 = 2, a 2 = 4 , h ( σ ) = tanh ( σ ) :‬תנאי‬ ‫של‬ ‫ההתחלה‬ ‫המשערך‬ ‫]‪xˆ ( 0 ) = 0 + ν1 ( 0 ) [ m ] , yˆ ( 0 ) = 0 + ν1 ( 0 ) [ m‬‬ ‫הם‪:‬‬ ‫] ‪ . θ̂ ( 0 ) = 20o [ deg‬מערכת הבקרה מבוססת על משוב מקום המדמה חיישן ‪ GPS‬עם‬ ‫רעש מדידה ‪ ν1 , ν 2‬לבן בעל ממוצע אפס ושונות ‪ , 0.1‬ומדידה )ללא רעש( של זווית‬ ‫הגלגלים הקדמיים )כיוון כלי הרכב אינו נמדד ומשוערך במסנן(‪ .‬נתונים נוספים של‬ ‫המסנן הם )ב‪ P0 -‬ניתן משקל גדול למשתנה המצב ) ‪ c ( t‬ו‪ s ( t ) -‬בגלל אפשרות של‬ ‫שגיאה התחלתית גדולה במשתנים אלה(‪:‬‬ ‫‪0 0‬‬ ‫‪0 0 ‬‬ ‫‪ 0.1 0 ‬‬ ‫‪, R=‬‬ ‫‪‬‬ ‫‪2 0‬‬ ‫‪ 0 0.1‬‬ ‫‪‬‬ ‫‪0 2‬‬ ‫‪ 0.5 0‬‬ ‫‪ 0 0. 5‬‬ ‫‪P0 = ‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫בגרף ‪ 4.1‬מוצג המסלול האמיתי של כלי הרכב‪ ,‬בגרף ‪ 4.3‬מוצגות שגיאות כלי הרכב‬ ‫ביחס למסלול הרצוי כפי שנמדדו ע"י הבקר )כלומר מבוססות על שיערוך(‪ .‬בגרף ‪4.4‬‬ ‫ניתן לראות את מדידת המקום הרועשת בהשוואה לשערוך המקום המתקבל‬ ‫מהמסנן ובגרף ‪ 4.5‬ניתן לראות את שיערוך משתני המצב ) ‪ ( x , y, θ‬בהשוואה‬ ‫למשתני המצב האמתיים‪.‬‬ ‫‪Trajectory‬‬ ‫‪trajectory‬‬ ‫‪desired trajectory‬‬ ‫‪10‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫]‪y [m‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪15‬‬ ‫‪5‬‬ ‫‪10‬‬ ‫‪0‬‬ ‫]‪x [m‬‬ ‫גרף ‪ - 4.1‬מסלול כלי הרכב )סימולציה עם בקר )‪ (2.2.27‬ומשערך )‪((4.1.8‬‬ ‫‪127‬‬ Control Inputs 2 v(t) 1.5 1 0.5 0 0 2 4 6 8 10 time [sec] 12 14 16 18 20 0 2 4 6 8 10 time [sec] 12 14 16 18 20 8 6 w(t) 4 2 0 -2 -4 ((4.1.8) ‫( ומשערך‬2.2.27) ‫ – אותות בקרה )סימולציה עם בקר‬4.2 ‫גרף‬ Controller Tracking Errors 1 e1(t) 0 -1 -2 -3 0 2 4 6 8 10 time [sec] 12 14 16 18 20 0 2 4 6 8 10 time [sec] 12 14 16 18 20 0 2 4 6 8 10 time [sec] 12 14 16 18 20 2 e2(t) 1 0 -1 0.5 e3(t) 0 -0.5 -1 -1.5 ((4.1.8) ‫( ומשערך‬2.2.27) ‫ – שגיאות מבוססות שערוך )סימולציה עם בקר‬4.3 ‫גרף‬ 128 Estimated States vs. Measured States 20 x(t) 15 10 5 0 measured x(t) estimated x(t) 0 2 4 6 8 10 time [sec] 12 14 16 18 20 10 8 y(t) 6 4 measured y(t) estimated y(t) 2 0 0 2 4 6 8 10 time [sec] 12 14 16 18 20 (2.2.27) ‫ ( לעומת מדידת מקום )סימולציה עם בקר‬x , y ) ‫ – שיערוך מקום‬4.4 ‫גרף‬ ((4.1.8) ‫ומשערך‬ Estimated States vs. Real States 20 x(t) 15 10 real x(t) estimated x(t) 5 0 0 2 4 6 8 10 time [sec] 12 14 16 18 20 10 y(t) 8 6 real y(t) estimated y(t) 4 2 0 2 4 6 8 10 time [sec] 12 14 16 18 20 100 θ(t) 50 real θ(t) estimated θ(t) 0 -50 0 2 4 6 8 10 time [sec] 12 14 16 18 20 ‫ ( לעומת מצב אמיתי )סימולציה עם בקר‬x , y,θ ) ‫ שיערוך מצב כלי הרכב‬- 4.5 ‫גרף‬ ((4.1.8) ‫( ומשערך‬2.2.27) 129 ‫‪Estimation Errors‬‬ ‫‪0.4‬‬ ‫‪0.2‬‬ ‫‪x - x est‬‬ ‫‪0‬‬ ‫‪-0.2‬‬ ‫‪20‬‬ ‫‪18‬‬ ‫‪16‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫]‪time [sec‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪20‬‬ ‫‪18‬‬ ‫‪16‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫]‪time [sec‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪20‬‬ ‫‪18‬‬ ‫‪16‬‬ ‫‪14‬‬ ‫‪12‬‬ ‫‪10‬‬ ‫]‪time [sec‬‬ ‫‪8‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪-0.4‬‬ ‫‪0.4‬‬ ‫‪0.2‬‬ ‫‪y - y est‬‬ ‫‪0‬‬ ‫‪-0.2‬‬ ‫‪-0.4‬‬ ‫‪40‬‬ ‫‪0‬‬ ‫‪θ - θest‬‬ ‫‪20‬‬ ‫‪-20‬‬ ‫גרף ‪ – 4.6‬שגיאות שיערוך )סימולציה עם בקר )‪ (2.2.27‬ומשערך )‪((4.1.8‬‬ ‫‪ – 4.2‬מימוש בקר המופעל בגישה של מערכת קואורדינאטות קרטזית קבועה‬ ‫במרחב‪.‬‬ ‫האלגוריתמים המוצגים בסעיף ‪ 2.2‬מיועדים לנהיגה אוטונומית של כלי רכב לאורך‬ ‫מסלול רצוי המוגדר ע"י פונקציה אנליטית על מערכת צירים קרטזית קבועה‬ ‫במרחב‪ .‬מכיוון שלא ניתן לייצג כל מסלול רצוי בשטח על ידי פונקציה יחידה‪ ,‬והדרך‬ ‫המקובלת לייצוג מסלול רצוי )במערכת הפועלת בתנאי שדה( היא על ידי שימוש‬ ‫בסדרה של "נקודות דרך" )‪ , ( xdi , ydi ) , i = 1K n (way points‬פותחה השיטה הבאה‪:‬‬ ‫‪ .1‬בכל "נקודת דרך" )מלבד ) ‪ ( ( xd n , yd n‬מוגדרת מערכת צירים מקומית כך‬ ‫שראשיתה בנקודה ) ‪ ( xdi , ydi‬וציר ‪ x‬שלה עובר דרך הנקודה ) ‪. ( xdi +1 , ydi +1‬‬ ‫‪ .2‬על כל מערכת צירים מקומית מתוכנן פולינום ממעלה חמישית המחבר את‬ ‫"נקודת הדרך" )ראשית מערכת הצירים( עם נקודת הדרך הבאה‪ ,‬ראה ) ‪fi ( x‬‬ ‫באיור ‪4.2.1‬‬ ‫‪130‬‬ ‫‪ .3‬הפולינומים )ממעלה חמישית( מתוכננים כך שתשמר רציפות במקום בכיוון‬ ‫ובעקמומיות )רציפות כזאת דרושה לפעולה חלקה של ההגה כאשר כלי הרכב‬ ‫נוסע על המסלול הרצוי(‪ .‬כדי להבטיח גם רציפות באות הבקרה ) &‪ ( w = φ‬יש‬ ‫להרחיב את השיטה כך שתשמר גם רציפות בקצב שינוי העקמומיות‪ ,‬לדוגמא‬ ‫ע"י שימוש בפולינומים ממעלה שביעית )הרחבה כזאת לא בוצעה בעבודה(‪.‬‬ ‫‪ .4‬פותח מנגנון אשר מאפשר לבקר כלי הרכב לקבוע בכל רגע את מערכת‬ ‫הצירים והפונקציה המקומית המהווים ייצוג רגעי של המסלול הרצוי )הבקר‬ ‫בכלי הרכב מקבל את נקודות הדרך ומבצע את כל הפעולות בזמן אמת(‪.‬‬ ‫מערכת‬ ‫צירים ‪i+1‬‬ ‫) ‪fi+1 ( x‬‬ ‫) ‪( xdi + 2 , ydi + 2‬‬ ‫) ‪( xdi +1 , ydi +1‬‬ ‫) ‪fi ( x‬‬ ‫מערכת‬ ‫צירים ‪i‬‬ ‫) ‪( xdi , ydi‬‬ ‫איור ‪ - 4.2.1‬מסלול המבוסס על פולינומים ממעלה ‪5‬‬ ‫חישוב הפולינומים המייצגים את המסלול הרצוי מבוסס על תכנון תנאי גבול‬ ‫מתאימים בכל נקודת דרך )מקום‪ ,‬כיוון ועקמומיות(‪ ,‬השיטה המשמשת לחישוב זה‬ ‫מוצגת בנספח ‪ .A.2‬ע"פ שיטה זו חישוב הפולינום ) ‪) fi ( x‬בקטע מספר ‪ (i‬מתבסס על‬ ‫הנקודות‪( xdi + 2 , ydi + 2 ) , ( xdi +1 , ydi +1 ) , ( xdi , ydi ) , ( xdi −1 , ydi −1 ) , ( xdi − 2 , yi − 2 ) :‬‬ ‫ו‪ . ( xdi + 3 , ydi + 3 ) -‬כלומר חישוב המסלול בכל נקודה לאורך המסלול הוא חישוב‬ ‫מקומי המבוסס על מספר קטן של נקודות‪ .‬עובדה זו מאפשרת תכנון ושינוי מסלול‬ ‫בזמן אמת הנובע מהוספה או הזזה של נקודות‪ ,‬לדוגמא בעקבות גילוי של‬ ‫מכשולים‪ .‬בזמן הנסיעה כלי הרכב נדרש לבחור בכל רגע את מערכת הצירים שביחס‬ ‫אליה הוא צריך לפעול‪ .‬הגורמים המשפיעים על בחירת מערכת צירים זו הם מקום‬ ‫כלי הרכב ביחס לנקודות המסלול וכיוונו ביחס לכיוון ההתקדמות הרצוי‪ .‬לכל‬ ‫מערכת צירים מוגדר אזור שייכות‪ ,‬אזור זה הינו הגזרה התחומה בין שני‬ ‫הנורמאלים למסלול בנקודות הדרך ‪ i‬ו‪) ( i + 1) -‬ראה איור ‪ ,(4.2.2‬חלוקה כזאת‬ ‫לאזורי שייכות מכסה בצורה טובה את השטח סביב המסלול‪.‬‬ ‫‪131‬‬ ‫גזרה ‪ i+1‬המתאימה‬ ‫למערכת צירים ‪i+1‬‬ ‫)ייצוג המסלול בין‬ ‫הנק' ‪ i+1‬ו‪(i+2 -‬‬ ‫גזרה ‪ i‬המתאימה‬ ‫למערכת צירים ‪i‬‬ ‫)ייצוג המסלול בין‬ ‫הנק' ‪ i‬ו‪(i+1 -‬‬ ‫גזרה ‪ i-1‬המתאימה‬ ‫למערכת צירים ‪i-1‬‬ ‫)ייצוג המסלול בין‬ ‫הנק' ‪ i-1‬ו‪(i -‬‬ ‫) ‪fi ( x‬‬ ‫) ‪fi+1 ( x‬‬ ‫) ‪fi-1 ( x‬‬ ‫איור ‪ - 4.2.2‬חלוקת השטח סביב המסלול לגזרות‬ ‫הערה‪ :‬שיטת הגזרות המתוארת כאן אינה מתאימה לשימוש כאשר ארבע נקודות‬ ‫דרך עוקבות נמצאות לאורך קו ישר‪ .‬השיטה פותחה כדי לתת מענה לבעיות‬ ‫הנובעות מקיום זווית מסוימת בין מערכות הצירים וכדי למנוע מצב של שטחים‬ ‫מתים )כאלה שאינם מכוסים ע"י מערכות הצירים( סביב המסלול‪.‬‬ ‫לפני תחילת הנסיעה כל מערכות הצירים נסרקות )באופן אוטומטי ע"י הבקר(‬ ‫ונבחרת זו שעבורה מתקיימים התנאים הבאים‪ .1 :‬שייכות לגזרה‪ .2 ,‬סכום שגיאת‬ ‫המקום והכיוון מינימאלי‪ .3 ,‬השגיאה בכיוון קטנה מ‪ 90 -‬מעלות‪.‬‬ ‫במהלך הנסיעה שינוי מערכת הצירים מתרחש כאשר כלי הרכב מגיעה לגבול הגזרה‬ ‫)עליון לנסיעה קדימה ותחתון לנסיעה אחורה(‪ .‬במהלך הנסיעה לא מתבצעת סריקה‬ ‫של מערכות הצירים‪ ,‬טכניקה זו מאפשרת התחלת נסיעה מכל נקודה לאורך‬ ‫המסלול )אשר נמצאת בתוך אזור הגזרות( ואינה יוצרת עומס חישובי במקרה של‬ ‫מערכות צירים רבות‪.‬‬ ‫כדי לקבל מדידה חלקה של שגיאות כלי הרכב ביחס למסלול הרצוי דרוש בקר‬ ‫המאפשר מדידה של המסלול בקואורדינאטה ‪ x‬שונה מזו של כלי הרכב‬ ‫)קואורדינאטה זו מסומנת באיור ‪ 4.2.3‬ע"י ‪ x‬כאשר לצורך הדוגמה כלי הרכב נמצא‬ ‫בגבול הגזרה(‪.‬‬ ‫‪132‬‬ ‫) ‪fi ( x‬‬ ‫‪x‬‬ ‫‪x‬‬ ‫איור ‪ - 4.2.3‬קואורדינאטת עזר ‪. x‬‬ ‫צורך זה נובע מכך שהפונקציה ) ‪ fi ( x‬מוגדרת אך ורק עבור קואורדינאטות ‪x‬‬ ‫הנמצאות בתחום אפס עד ‪ . l i‬לדוגמא כאשר כלי הרכב נמצא בגבול הגזרה )כמו‬ ‫באיור ‪ (4.2.3‬הנקודה ) ‪ fi ( x‬אינה שייכת למסלול הרצוי )ויש למדוד בנקודה ) ‪.( fi ( x‬‬ ‫תכנון הקואורדינאטה ‪ x‬מתבסס על הדרישות הבאות‪:‬‬ ‫‪ .1‬כאשר כלי הרכב נמצא על המסלול הרצוי אז מתקיים ‪. x = x‬‬ ‫‪ .2‬כאשר כלי הרכב נמצאה בגבול גזרה העובר דרך נקודה דרך מסוימת‪ ,‬לדוגמא‬ ‫‪ , xd i‬אז מתקיים ‪) x = xd i‬גם אם כלי הרכב לא נמצא על המסלול הרצוי‪,‬‬ ‫ראה לדוגמא איור ‪.(4.2.3‬‬ ‫הדרך שנבחרה כאן לתכנון הקואורדינאטה ‪ x‬נתונה ב‪:(4.2.1) -‬‬ ‫)‪(4.2.1‬‬ ‫) ‪x = x + e1 tan ( α‬‬ ‫כאשר ‪ e1 = f ( x ) − y‬שגיאת המקום של כלי הרכב‪ ,‬והזווית ‪ α‬היא הזווית בין קו‬ ‫מרכזי וקו מלווה הנתונים באיור ‪.4.2.4‬‬ ‫‪133‬‬ ‫‪α‬‬ ‫קו מלווה‬ ‫קו מרכזי‬ ‫איור ‪ - 4.2.4‬תכנון הקואורדינאטה ‪x‬‬ ‫ומוגדרים באופן הבא‪:‬‬ ‫קו מרכזי – קו העובר דרך הנקודה בה נפגשים הנורמאלים למסלול בנקודות ‪ i‬ו‪i+1 -‬‬ ‫וניצב לציר ‪ x‬של מערכת הצירים המקומית‪.‬‬ ‫קו מלווה ‪ -‬קו העובר דרך הנקודה בה נפגשים הנורמאלים למסלול בנקודות ‪ i‬ו‪i+1 -‬‬ ‫ודרך הנקודה ) ‪ ( x , y‬שבה נמצא כלי הרכב‪.‬‬ ‫חישוב ‪ x‬מצריך פתרון של המשוואה האלגברית הבאה‪:‬‬ ‫)‪(4.2.2‬‬ ‫) ‪x = x + ( f ( x ) − y ) tan ( α‬‬ ‫פתרון המשוואה )‪ (4.2.2‬בזמן אמת מתקבל ע"י שימוש בשיטת קירוב נומרית‪.‬‬ ‫מימוש בקר בגישה של מערכת קואורדינאטות קבועה במרחב כאשר השטח סביב‬ ‫המסלול מחולק לגזרות בשיטה שתוארה אפשרי בשלוש דרכים )הראשונה אינה‬ ‫כוללת בסיס מתמטי והשתיים האחרות בעלות בסיס מתמטי איתן(‪:‬‬ ‫‪ .1‬ניתן לממש את הבקרים )‪ (2.2.6‬או )‪ (2.2.27‬כאשר המסלול הרצוי נמדד בנקודה‬ ‫‪x‬‬ ‫במקום בנקודה ‪) x‬כלומר יש להחליף את‬ ‫) ‪f ( x ) , f ′ ( x ) , f ′′ ( x‬‬ ‫עם‬ ‫) ‪ .( f ( x ) , f ′ ( x ) , f ′′ ( x‬שינוי זה‪ ,‬במקרה של בקרים )‪ (2.2.6‬או )‪ (2.2.27‬מביא לכך‬ ‫שמודל החוג הסגור אינו בצורת שרשרת ולכן הבסיס המתמטי של בקרים אלה כפי‬ ‫שנדון בפרק ‪ 2‬אינו תקף‪ .‬למרות עובדה זו בסימולציות וניסויים שבצענו )ראה‬ ‫תוצאות ניסוי בסעיף זה( כלי הרכב התכנס למסלול והתקבלו ביצועים מספקים‪.‬‬ ‫‪134‬‬ ‫‪ .2‬ניתן לממש את בקר )‪ (2.2.67‬כאשר‪:‬‬ ‫&‪α‬‬ ‫⇒‬ ‫‪cos 2 α‬‬ ‫‪x& − y& tan α‬‬ ‫&‪e1α‬‬ ‫= &‪x‬‬ ‫‪+‬‬ ‫‪2‬‬ ‫) ‪(1 − f ′ ( x ) tan α ) cos α (1 − f ′ ( x ) tan α‬‬ ‫‪x& = x& + ( f ′ ( x ) x& − y& ) tan α + e1‬‬ ‫)‪(4.2.3‬‬ ‫‪ cos θ − sin θ tan α‬‬ ‫‪‬‬ ‫‪e1 cos ϕ‬‬ ‫‪=‬‬ ‫‪+‬‬ ‫) ‪ v = q1 ( t ) v ( t‬‬ ‫‪2‬‬ ‫‪′‬‬ ‫‪′‬‬ ‫‪ (1 − f ( x ) tan α ) r cos α (1 − f ( x ) tan α ) ‬‬ ‫‪ r‬מרחק כלי הרכב מהנקודה בה נפגשים הנורמאלים למסלול בנקודות ‪ i‬ו‪ i+1 -‬ו‪ϕ -‬‬ ‫כיוון כלי הרכב ביחס לקו ייחוס הניצב לקו המלווה )‪ ϕ ,r‬ו‪ α -‬מוגדרים כאן כמו‬ ‫במודל הפולארי )‪.(2.3.1‬‬ ‫המצב הסינגולארי ‪ 1 − f ′ ( x ) tan α = 0‬הוא תולדה של מקרה בו כיוון המסלול הרצוי‬ ‫בנקודה ‪ x‬שווה לכיוון הקו המלווה )במצב כזה &‪ x‬אינו מוגדר(‪ .‬מצב סינגולארי‬ ‫נוסף קיים כאשר ‪ cos α = 0‬אך מצב כזה אינו אפשרי בשיטה המוצעת כאן לתכנון‬ ‫מסלול‪.‬‬ ‫‪.3‬‬ ‫ניתן לממש את בקר )‪) (2.3.9‬בקר בגישה של מערכת קואורדינאטות פולארית‬ ‫קבועה במרחב( כאשר הקו המלווה ו‪ α -‬הנתונים באיור )‪ (4.2.4‬הם משתני המצב ‪r‬‬ ‫ו‪ α -‬הנתונים באיור ‪) 2.3.1‬מודל כלי רכב בעל ארבעה גלגלים במערכת‬ ‫קואורדינאטות פולארית(‪ .‬בקר זה יותר קל למימוש בהשוואה לבקר )‪) (2.2.67‬לא‬ ‫מצריך פתרון של )‪ ((4.2.2‬אבל הוא מצריך ייצוג של המסלול הרצוי במערכת‬ ‫קואורדינאטות פולארית‪ .‬במקרה כזה ניתן לפתח שיטה דומה לשיטה המוצגת‬ ‫בנספח ‪ A.2‬כאשר המסלול מורכב מפולינומים ) ‪ rd = fi ( α‬המוגדרים במערכת‬ ‫קואורדינאטות פולארית‪.‬‬ ‫השיטה המוצגת בנספח ‪ A.2‬פותחה כך שבקר כלי הרכב יוכל לבצע את תכנון‬ ‫המסלול בין "נקודות הדרך" בזמן אמת‪ ,‬כלומר בקר כלי הרכב מקבל מהמפעיל רק‬ ‫את "נקודות הדרך"‪ .‬כדי שהמפעיל יוכל לדעת מראש האם הדרך שבה בקר כלי‬ ‫הרכב יבחר לחבר את "נקודות הדרך" היא מתאימה מבחינת מכשולים ועבירות‬ ‫)ביחס לנתונים הקיימים במפה או בתצלום אוויר( פותחה אפליקציה מבוססת‬ ‫‪ Matlab‬אשר פועלת באופן הבא‪:‬‬ ‫‪ .1‬מפה או תצלום אוויר מוצגים על מסך המחשב‪.‬‬ ‫‪ .2‬המפעיל מסמן את "נקודות דרך" הרצויות באמצעות העכבר‪.‬‬ ‫‪ .3‬האפליקציה מחשבת ומציגה על המסך את המסלול שבו בקר כלי הרכב יבחר‬ ‫לחבר את "נקודות הדרך"‪.‬‬ ‫‪135‬‬ ‫‪ .4‬אם המסלול אינו מתאים‪ ,‬המפעיל יכול באופן אינטראקטיבי להזיז את‬ ‫"נקודות הדרך" ולראות כיצד המסלול משתנה בהתאם‪.‬‬ ‫‪ .5‬האפליקציה לא מאפשרת סימון נקודות אשר יביאו לתכנון מסלול אשר לא‬ ‫מתאים לרכב מבחינה גיאומטרית )לדוגמה מצריך סיבוב הגה לזווית לא‬ ‫אפשרית(‪ .‬ניתן בעתיד להוסיף גם מגבלות הקשורות לשיפועים של השטח‬ ‫)אם קיימים נתוני גובה(‬ ‫‪ .6‬בסוף תהליך התכנון האפליקציה טוענת את "נקודות הדרך" בבקר כלי הרכב‪.‬‬ ‫למ ְנ ַשק האפליקציה )‪ (interface‬ניתן לראות באיור ‪ ,4.2.5‬כאשר מוצג בו‬ ‫דוגמא ִ‬ ‫מסלול המורכב מ‪" 14 -‬נקודות דרך"‪ .‬המסלול המוצג שימש בניסוי שהתקיים בחנייה‬ ‫של אוניברסיטת בן‪-‬גוריון בנגב ומומשו בו‪ :‬בקר )‪) (2.3.6‬לנהיגה אוטונומית לאורך‬ ‫מסלול רצוי בגישה של מערכת קואורדינאטות קרטזית קבועה במרחב(‪ ,‬מסנן ‪,EKF‬‬ ‫אלגוריתם לתכנון מסלול ושינוי מערכות קואורדינאטות בזמן אמת‪ .‬הניסוי בוצע‬ ‫לפני שפותחו בקרים )‪ (2.2.67) ,(2.2.27‬ו‪ (2.3.9) -‬ומודל )‪ (4.1.7‬המאפשר שימוש‬ ‫במסנן קלמן )לא מורחב(‪ .‬תוצאות הניסוי מוצגות בגרפים ‪ 4.7‬עד ‪.4.10‬‬ ‫‪End = Esc , Import Points = I , Delete nearest point = Del ,Add Point = Left Click , Add middle Point = Shift+(Left Click) , Undo Move = U‬‬ ‫‪Move Nearest Point = Right Click , Zoom In = + , Zoom Out = - , Auto Zoom = 0 , Move: Up, Down, Left, Right with the Arrows‬‬ ‫איור ‪ִ – 4.2.5‬מ ְנ ַשק אפליקציה לתכנון מסלול‪ ,‬והצגת המסלול הרצוי בניסוי‪.‬‬ ‫‪136‬‬ ‫בגרף ‪ 4.5‬מוצגות שגיאות העקיבה )כפי שמוגדרות ב‪ ((2.2.1 -‬ניתן לראות ששגיאת‬ ‫המקום ההתחלתית היא כ‪ 1 -‬מטר ושאחרי התכנסות היא אינה גדולה מ‪ 0.5 -‬מטר‬ ‫לאורך כל המסלול‪.‬‬ ‫בגרף ‪ 4.6‬מוצגים המסלול הרצוי ונקודות הדרך )‪ (way points‬בהשוואה למסלול שכלי‬ ‫הרכב נסע כפי ששוערך ע"י ממסנן הקלמן המורחב‪.‬‬ ‫בגרף ‪ 4.7‬מוצג כיוון כלי הרכב כפי שנמדד על ידי המצפן בהשוואה לשערוך הכיוון‬ ‫ע"י המסנן‪ .‬ניתן לראות כי מדידות המצפן רועשות מאוד )לכן אינן מתאימות‬ ‫לשימוש עם אלגוריתם הבקרה( בהשוואה לתוצאות השערוך )המבוססות על‬ ‫מדידות ה‪ GPS -‬וכיוון הגלגלים הקדמיים(‪.‬‬ ‫בגרף ‪ 4.8‬מוצגות מדידות המקום של ה‪ GPS -‬בהשוואה לשערוך המקום מהמסנן‪,‬‬ ‫בהגדלה המצורפת ניתן לראות שתוצאות המסנן חלקות יותר בהשוואה למדידות‬ ‫הגולמיות המתקבלות ממכשיר ה‪.GPS -‬‬ ‫‪Trajectory Errors‬‬ ‫‪0‬‬ ‫]‪e1 [m‬‬ ‫‪-0.5‬‬ ‫‪-1‬‬ ‫‪200‬‬ ‫‪150‬‬ ‫‪100‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫‪0‬‬ ‫‪200‬‬ ‫‪150‬‬ ‫‪100‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫‪0‬‬ ‫‪200‬‬ ‫‪150‬‬ ‫‪100‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫‪0‬‬ ‫‪0.6‬‬ ‫‪0.4‬‬ ‫‪e2‬‬ ‫‪0.2‬‬ ‫‪0‬‬ ‫‪-0.2‬‬ ‫‪0.5‬‬ ‫‪e3‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫גרף ‪ – 4.7‬שגיאות עקיבה )תוצאות ניסוי עם בקר )‪.((2.2.6‬‬ ‫‪137‬‬ ‫‪trajectory‬‬ ‫‪460340‬‬ ‫‪EKF trajectory‬‬ ‫‪Way Points‬‬ ‫‪desired trajectory‬‬ ‫‪460330‬‬ ‫‪460320‬‬ ‫‪460310‬‬ ‫‪460300‬‬ ‫‪460290‬‬ ‫‪671790‬‬ ‫‪671780‬‬ ‫‪671770‬‬ ‫‪671760‬‬ ‫‪671750‬‬ ‫‪671730‬‬ ‫‪671740‬‬ ‫‪460280‬‬ ‫‪671720‬‬ ‫גרף ‪ – 4.8‬המסלול הרצוי "ונקודות הדרך" בהשוואה למסלול כלי הרכב כפי ששוערך‬ ‫על ידי ה‪) EKF -‬תוצאות ניסוי עם בקר )‪.((2.2.6‬‬ ‫‪Comparison Between θ COMPASS and θ EKF‬‬ ‫‪0‬‬ ‫‪-100‬‬ ‫]‪θ COMPASS [deg‬‬ ‫‪100‬‬ ‫‪-200‬‬ ‫‪200‬‬ ‫‪150‬‬ ‫‪100‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫‪0‬‬ ‫‪200‬‬ ‫‪150‬‬ ‫‪100‬‬ ‫]‪time [sec‬‬ ‫‪50‬‬ ‫‪0‬‬ ‫‪100‬‬ ‫‪-100‬‬ ‫]‪θ EKF [deg‬‬ ‫‪0‬‬ ‫‪-200‬‬ ‫גרף ‪ – 4.9‬כיוון כלי הרכב כפי שנמדד על ידי המצפן בהשוואה לשערוך הכיוון של ה‪-‬‬ ‫‪) EKF‬תוצאות ניסוי עם בקר )‪.((2.2.6‬‬ ‫‪138‬‬ ‫‪trajectory‬‬ ‫‪460340‬‬ ‫‪GPS‬‬ ‫‪EKF‬‬ ‫‪460330‬‬ ‫‪460320‬‬ ‫‪460310‬‬ ‫‪460300‬‬ ‫‪460290‬‬ ‫‪671790‬‬ ‫‪671780‬‬ ‫‪671770‬‬ ‫‪671760‬‬ ‫‪671750‬‬ ‫‪671740‬‬ ‫‪671730‬‬ ‫‪460280‬‬ ‫‪671720‬‬ ‫הגדלה‬ ‫‪trajectory‬‬ ‫‪460306.5‬‬ ‫‪GPS‬‬ ‫‪EKF‬‬ ‫‪460306‬‬ ‫‪460305.5‬‬ ‫‪671735‬‬ ‫‪671730‬‬ ‫‪460305‬‬ ‫‪671725‬‬ ‫גרף ‪– 4.10‬מדידות המקום של ה‪ GPS -‬בהשוואה לשערוך המקום של ה‪EKF -‬‬ ‫)תוצאות ניסוי עם בקר )‪.((2.2.6‬‬ ‫‪139‬‬ ‫‪ – 4.3‬מימוש בקר המופעל בגישה של מערכת קואורדינאטות מקומיות‬ ‫בסעיף זה מוצגים כלים המתאימים למימוש הבקרים הנתונים בסעיף ‪ .2.4‬בקרים‬ ‫אלה מתאימים למקרה בו המסלול הרצוי נמדד במערכת קואורדינאטות מקומית‪,‬‬ ‫לדוגמה מערכת קואורדינאטות רתומה לכלי הרכב‪ .‬באופן כללי בקרים אלה נועדו‬ ‫למקרה בו המסלול הרצוי אינו ידוע מראש ונמדד בזמן אמת ע"י חיישנים )כגון‬ ‫מצלמה( המותקנים על כלי הרכב‪ .‬במקרה כזה אין צורך בייצוג של המסלול או מצב‬ ‫כלי הרכב במערכת קואורדינאטות קבועה במרחב‪ .‬ניתן להפעיל את הבקרים‬ ‫הנתונים בסעיף ‪ 2.4‬גם בגישה אחרת אשר בה המסלול הרצוי נתון במערכת‬ ‫קואורדינאטות קבועה במרחב ומבוצעת טרנספורמציה בזמן אמת של המסלול‬ ‫למערכת הקואורדינאטות המקומית‪ ,‬במקרה כזה ניתן לממש את הבקרים עם חיישן‬ ‫כגון ‪ .GPS‬בניגוד לשיטות שהוצגו בסעיף ‪ 4.2‬ונועדו למימוש הבקרים הנתונים‬ ‫בסעיף ‪ 2.2‬כאן אין צורך בייצוג המסלול הרצוי ע"י פונקציות המוגדרות על מערכות‬ ‫צירים מקומיות )נייחות( ולא נדרש תהליך של מעבר בין מערכות צירים )נייחות(‪.‬‬ ‫לעומת זאת במקרה של הבקרים הנתונים בסעיף ‪ ,2.4‬חישוב נקודת הייחוס על‬ ‫המסלול הרצוי בדרך כלל מורכב יותר ומצריך שימוש בקרובים נומריים )לדוגמא‬ ‫במקרה של בקר )‪ (2.4.17‬יש לחשב את נקודת החיתוך בין המסלול הרצוי ובין ציר ‪x‬‬ ‫של מערכת הקואורדינאטות הרתומה לכלי הרכב(‪.‬‬ ‫מימוש הבקרים הנתונים בסעיף ‪ 2.4‬מצריך מסלול רציף בעל תכונות גיאומטריות‬ ‫מתאימות‪ .‬כמו בסעיף ‪ 4.2‬גם כאן מוצגת שיטה המאפשרת תכנון מסלול מתאים‬ ‫העובר דרך סדרה של נקודות דרך רצויות ‪ .(way points) xd i , yd i‬במקרה זה תכנון‬ ‫המסלול מבוסס על עקומים פרמטריים מהצורה הבא‪:‬‬ ‫)‪(4.3.1‬‬ ‫‪f x i ( s ) = a x s7 + b x s 6 + c x s5 + d x s 4 + e x s3 + f x s 2 + g x s + h x‬‬ ‫‪f yi ( s ) = a y s 7 + b y s 6 + c y s 5 + d y s 4 + e y s 3 + f y s 2 + g y s + h y‬‬ ‫כאשר העקום ב‪ (4.3.1) -‬מחבר את נקודות הדרך ‪ xdi , yd i‬עם ‪) xd i +1 , yd i +1‬ראה איור‬ ‫‪ A.3.1‬בנספח ‪.(A.3‬‬ ‫שיטת התכנון של הפולינומים )‪ (4.3.1‬מוצגת בנספח ‪ A.3‬ופותחה כך שתשמר‬ ‫רציפות במקום‪ ,‬בכיוון‪ ,‬בעקמומיות ובקצב שינוי העקמומיות )רציפות כזאת‬ ‫מאפשרת פעולה חלקה של אות בקרה מהצורה ) ‪.( w ( t ) = φ& ( t‬‬ ‫‪140‬‬ ‫בגרף ‪ 4.11‬מוצג מסלול רצוי העובר דרך ‪ 14‬נקודות דרך )ותוכנן לפי השיטה הנתונה‬ ‫בנספח ‪ .(A.3‬בגרף גם מוצגות תוצאות של סימולציה עם בקר בגישה של מערכת‬ ‫קואורדינאטות מקומית רתומה לכלי הרכב )‪.(2.4.17‬‬ ‫אורך כלי הרכב בסימולציה הוא ‪ , 1m‬קבועי הבקר הם ‪ a 1 = a 2 = k = 2‬ותנאי התחלה‬ ‫הם ‪) x ( 0 ) = 0 [ m ] , y ( 0 ) = 5 [ m ] , θ ( 0 ) = 90 [ deg ] , φ ( 0 ) = 0‬כלי הרכב המצויר באיור‬ ‫מסמן את תנאי ההתחלה(‪.‬‬ ‫בגרפים ‪ 4.12‬ו‪ 4.13 -‬מוצגים אותות הבקרה ושגיאות כלי הרכב ביחס למסלול‬ ‫הרצוי‪.‬‬ ‫‪30‬‬ ‫‪way points‬‬ ‫‪desired trajectory‬‬ ‫‪car trajectory‬‬ ‫‪10‬‬ ‫‪7‬‬ ‫‪9‬‬ ‫‪8‬‬ ‫‪11‬‬ ‫‪25‬‬ ‫‪12‬‬ ‫‪20‬‬ ‫‪3‬‬ ‫‪6‬‬ ‫‪15‬‬ ‫‪2‬‬ ‫]‪Y [m‬‬ ‫‪13‬‬ ‫‪10‬‬ ‫‪4‬‬ ‫‪5‬‬ ‫‪5‬‬ ‫‪1‬‬ ‫‪16‬‬ ‫‪30‬‬ ‫‪25‬‬ ‫‪20‬‬ ‫‪-5‬‬ ‫‪14‬‬ ‫‪15‬‬ ‫‪15‬‬ ‫‪10‬‬ ‫]‪X [m‬‬ ‫‪5‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪-5‬‬ ‫‪-10‬‬ ‫‪-10‬‬ ‫גרף ‪ – 4.11‬מסלול הנסיעה של כלי הרכב ביחס למסלול הרצוי )סימולציה‬ ‫עם בקר )‪ (2.4.17‬ותכנון מסלול בשיטה המוצגת בנספח ‪(A.3‬‬ ‫‪141‬‬ ‫‪1‬‬ ‫‪0.8‬‬ ‫]‪v(t) [m/sec‬‬ ‫‪0.6‬‬ ‫‪0.4‬‬ ‫‪0.2‬‬ ‫‪180‬‬ ‫‪160‬‬ ‫‪140‬‬ ‫‪120‬‬ ‫‪100‬‬ ‫‪180‬‬ ‫‪160‬‬ ‫‪140‬‬ ‫‪120‬‬ ‫‪100‬‬ ‫‪80‬‬ ‫‪60‬‬ ‫‪40‬‬ ‫‪20‬‬ ‫‪0‬‬ ‫‪80‬‬ ‫‪60‬‬ ‫‪40‬‬ ‫‪20‬‬ ‫‪0‬‬ ‫‪0‬‬ ‫‪1.5‬‬ ‫‪1‬‬ ‫]‪w(t) [rad/sec‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫‪-1‬‬ ‫]‪t [sec‬‬ ‫גרף ‪ - 4.12‬אותות בקרה )סימולציה עם בקר )‪ (2.4.17‬ותכנון מסלול בשיטה המוצגת‬ ‫בנספח ‪(A.3‬‬ ‫‪6‬‬ ‫‪4‬‬ ‫‪e1‬‬ ‫‪2‬‬ ‫‪0‬‬ ‫‪180‬‬ ‫‪160‬‬ ‫‪140‬‬ ‫‪120‬‬ ‫‪100‬‬ ‫‪80‬‬ ‫‪60‬‬ ‫‪40‬‬ ‫‪20‬‬ ‫‪0‬‬ ‫‪180‬‬ ‫‪160‬‬ ‫‪140‬‬ ‫‪120‬‬ ‫‪100‬‬ ‫‪80‬‬ ‫‪60‬‬ ‫‪40‬‬ ‫‪20‬‬ ‫‪0‬‬ ‫‪180‬‬ ‫‪160‬‬ ‫‪140‬‬ ‫‪120‬‬ ‫‪100‬‬ ‫‪80‬‬ ‫‪60‬‬ ‫‪40‬‬ ‫‪20‬‬ ‫‪0‬‬ ‫‪-2‬‬ ‫‪0.5‬‬ ‫‪0‬‬ ‫‪e2‬‬ ‫‪-0.5‬‬ ‫‪-1‬‬ ‫‪-1.5‬‬ ‫‪0.5‬‬ ‫‪e3‬‬ ‫‪0‬‬ ‫‪-0.5‬‬ ‫]‪t [sec‬‬ ‫גרף ‪ – 4.13‬שגיאות ביחס למסלול הרצוי )סימולציה עם בקר )‪ (2.4.17‬ותכנון מסלול‬ ‫בשיטה המוצגת בנספח ‪(A.3‬‬ ‫‪142‬‬ ‫נספח ‪ – A.1‬תיאור מערכת הניסוי‬ ‫לצורך מימוש וניסוי )בתנאי שדה( של האלגוריתמים אשר הוצגו בעבודה המחקר‪,‬‬ ‫פותח ונבנה כלי רכב בעל הנעה חשמלית )ראה איור ‪ A.1.1‬וטבלה ‪ .(A.1.1‬בנספח זה‬ ‫מוצגים מבנה כלי הרכב ומערכות החישוב‪ ,‬המפעילים והחיישנים המותקנים בו‪.‬‬ ‫אנטנת ‪GPS‬‬ ‫‪GPS‬‬ ‫מצפן‬ ‫משדר מקלט אלחוטי‬ ‫כרטיס בקרה של‬ ‫חברת ‪dSPACE‬‬ ‫מאפשר הפעלת קוד‬ ‫המתקבל מתוכנת‬ ‫‪.Matlab/Simulink‬‬ ‫מחשב נייד‬ ‫דוחף מנוע אחורי‬ ‫פועל בחוג מהירות‬ ‫מנוע חשמלי משמש‬ ‫להפעלת ההגה‬ ‫מנוע חשמלי וגיר‬ ‫משמשים להפעלת‬ ‫הגלגלים האחוריים‬ ‫דוחף מנוע קדמי )הגה(‬ ‫פועל בחוג מהירות‬ ‫אנקודרים‬ ‫איור ‪ – A.1.1‬כלי רכב חשמלי‪ ,‬מיועד לניסוי מערכות הבקרה בתנאי שדה )‪.(outdoor‬‬ ‫מאפיין‬ ‫כמות‬ ‫יחידות‬ ‫אורך‪:‬‬ ‫‪1.55‬‬ ‫]‪[m‬‬ ‫רוחב‪:‬‬ ‫‪0.76‬‬ ‫]‪[m‬‬ ‫גובה )ללא אנטנות(‪:‬‬ ‫‪1.35‬‬ ‫]‪[m‬‬ ‫גובה מרכז כובד‪:‬‬ ‫‪0.45‬‬ ‫]‪[m‬‬ ‫‪) l1‬ראה איור ‪(3.2.2‬‬ ‫‪35.5‬‬ ‫]‪[m‬‬ ‫‪) l2‬ראה איור ‪(3.2.2‬‬ ‫‪62.5‬‬ ‫]‪[m‬‬ ‫משקל‪:‬‬ ‫‪156‬‬ ‫]‪[kg‬‬ ‫מהירות מקסימאלית‪:‬‬ ‫כ‪3 -‬‬ ‫]‪[m/sec‬‬ ‫טבלה ‪ – A.1.1‬מאפייני כלי הרכב )מערכת הניסוי(‬ ‫‪143‬‬ ‫כלי הרכב מופעל באמצעות שני מנועי ‪ DC‬המחוברים לדוחפים )‪ (drivers‬המופעלים‬ ‫בחוג מהירות )האות בכניסה לדוחף הינו אות מהירות(‪ .‬המנוע הראשון מסובב את‬ ‫הגלגלים האחוריים וקובע את המהירות הקווית ‪ v‬והמנוע השני מסובב את ההגה‬ ‫וקובע את קצב סיבוב ההגה &‪. φ‬‬ ‫כדי לאפשר מדידה של מצב כלי הרכב בזמן אמת מותקנים בו החיישנים הבאים‪:‬‬ ‫ארבעה אנקודרים אינקרמנטליים בעלי ‪) 1000 ppr‬גלגלים אחוריים ימין ושמאל‪,‬‬ ‫מנוע אחורי ומנוע קדמי(‪ ,‬מצפן מגנטי ו‪ GPS (Global Position System) -‬דיפרנציאלי‪.‬‬ ‫שני האנקודרים המותקנים על הגלגלים האחוריים מאפשרים חישוב מצב כלי הרכב‬ ‫ע"י שימוש באודומטר‪ ,‬לשיטה זו חסרונות רבים בניהם שגיאה מצטברת )לא‬ ‫חסומה(‪ .‬לפיכך שיטה זו שימשה למדידת מצב כלי הרכב רק בניסויים הראשונים‬ ‫לפני התקנת מערכת ‪ ,GPS‬בגלל יתרונות מסוימים שיש לשימוש באנקודרים כגון‬ ‫חלקות וחסינות מפני רעשים יתכן וניתן בעתיד לשלבם עם חיישנים אחרים‪.‬‬ ‫האנקודר המותקן על המנוע האחורי )לפני הגיר( משמש למדידת המהירות הקווית‬ ‫והאנקודר המותקן על המנוע הקדמי משמש למדידת זווית ההגה ‪ φ‬ומהווה גם משוב‬ ‫לחוג המהירות בדוחף‪.‬‬ ‫בכלי הרכב מותקן חיישן מקום מסוג ‪ GPS‬של חברת ‪ .OmniSTAR‬חיישן זה פועל‬ ‫בשיטה דיפרנציאלית ולכן מאפשר דיוק טוב )כ‪ 0.5 -‬מטר( בהשוואה ל‪ GPS -‬רגיל‪.‬‬ ‫בשונה ממערכות ‪ GPS‬דיפרנציאליות רגילות אשר מצריכות שימוש בשני מכשירי‬ ‫‪ GPS‬וערוץ תקשורת אלחוטי‪ ,‬כאן מתקבלים תיקוני המקום באמצעות תקשורת‬ ‫לוויינית מתחנות קרקע קבועות שמפעילה חברת ‪ .OmniSTAR‬היתרון של שימוש‬ ‫במערכת זו בהשוואה ל‪ GPS -‬דיפרנציאלי רגיל הוא בהיותה עצמאית לגמרי במובן‬ ‫זה שאינה תלויה בקשר רדיו בין שני מכשירי ‪ GPS‬ולכן אינה מוגבלת בתווך‪.‬‬ ‫התקשורת בין מכשיר ה‪ GPS -‬ומערכת החישוב של כלי הרכב מבוססת ערוץ ‪rs-232‬‬ ‫ושימוש בפרוטוקול ‪ .(GGA message) NMEA-0183‬מערכת החישוב גם מבצעת‬ ‫המרה של הקריאה המתקבלת ממכשיר ה‪ GPS -‬ממעלות אורך ורוחב )‪Latitude-‬‬ ‫‪ (Longitude‬לקואורדינאטות )‪ .UTM (Universal Transverse Mercator‬הנוסחאות‬ ‫המשמשות כאן לצורך המרה זו נתונות ב‪ [DMATM 1989] -‬ודיוקן ‪.0.01m‬‬ ‫בנוסף מותקן בכלי הרכב מצפן מגנטי המחובר למערכת החישוב בערוץ אנלוגי‪ .‬עקב‬ ‫דיוק נמוך של מערכת זו המתבטא ברעש מדידה גבוה ורגישות לתנאי סביבה הוא‬ ‫אינו משמש למדידה רציפה של כיוון כלי הרכב אלא מספק כיוון התחלתי )תנאי‬ ‫התחלה( למסנן קלמן )אשר משמש גם כמשערך כיוון(‪.‬‬ ‫‪144‬‬ ‫מערכת החישוב של כלי הרכב מבוססת על כרטיס בקרה מסוג ‪ ds1103‬של חברת‬ ‫‪ .dSPACE‬הכרטיס כולל מעבד ‪ MPC 620‬של חברת ‪ Motorola‬הפועל בקצב ‪400 Mhz‬‬ ‫וערוצי קלט פלט רבים כגון‪ rs-232 ,Encoder Interface ,ADC ,DAC ,Digital IO :‬ועוד‪.‬‬ ‫הכרטיס מותקן בתוך קופסה קשיחה המותאמת לתנאי שדה‪ ,‬משוככת ע"י בולמי‬ ‫זעזועים וכוללת מסנן אבק‪ .‬כרטיס הבקרה מחובר דרך ערוץ תקשורת קווי למחשב‬ ‫נייד המותקן בחלקו הקדמי של כלי הרכב‪ .‬המחשב הנייד משמש לטעינת קוד‬ ‫בכרטיס הבקרה וכממשק משתמש המאפשר שינוי פרמטרים והצגת אותות בזמן‬ ‫אמת‪ .‬כרטיס הבקרה המותקן בכלי הרכב מאפשר טעינת קוד ישירות מתוך תוכנת‬ ‫‪ Matlab/Simulink‬ואינו מצריך תכנות בשפת ‪ .C‬יתרון זה מאפשר מימוש מיידי של‬ ‫אלגוריתמים אשר נבדקו בתנאי סימולציה )בתוכנת ה‪ (Matlab/Simulink -‬וחוסך זמן‬ ‫פיתוח רב‪.‬‬ ‫בכלי הרכב מותקנת גם יחידה לשליטה מרחוק המבוססת על ערוץ תקשורת אלחוטי‬ ‫)בעל עוצמת שידור מקסימאלית בשיעור ‪ .( 2 Watt‬תפקידה העיקרי של מערכת זו‬ ‫הוא עצירת כלי הרכב כאשר במהלך ניסוי נוצר מצב מסוכן )לכלי הרכב או לסביבה(‪.‬‬ ‫‪145‬‬ ‫נספח ‪ – A.2‬תכנון מסלול רצוי המתאים לבקרים בגישה של מערכת‬ ‫קואורדינאטות קרטזית קבועה במרחב‪.‬‬ ‫בסעיף זה מוצג תכנון מסלול רצוי העובר דרך סדרה של נקודות דרך רצויות‬ ‫ומתאים למימוש בקרים בגישה של מערכת קואורדינאטות קבועה במרחב‪.‬‬ ‫בין כל שתי נקודות דרך עוקבות מוגדרת מערכת צירים מקומית ופולינום ממעלה‬ ‫חמישית )ראה איור ‪ A.2.1‬ומשוואה ‪.(A.2.1‬‬ ‫‪l i +1‬‬ ‫) ‪( xdi + 2 , ydi + 2‬‬ ‫מערכת‬ ‫צירים ‪i+1‬‬ ‫) ‪fi +1 ( x‬‬ ‫‪ϕi +1‬‬ ‫) ‪( xdi +1 , ydi +1‬‬ ‫‪li‬‬ ‫) ‪fi ( x‬‬ ‫מערכת‬ ‫צירים ‪i‬‬ ‫‪ϕi‬‬ ‫) ‪( xdi , ydi‬‬ ‫איור ‪ - A.2.1‬מסלול המבוסס על פולינומים ממעלה ‪5‬‬ ‫הפולינום ) ‪ fi ( x‬מחבר את הנקודות ) ‪ (xd i , yd i‬ו‪ ( xdi +1 , ydi +1 ) -‬ונתון ע"י‪:‬‬ ‫)‪(A.2.1‬‬ ‫‪f i ( x ) = a i x 5 + b i x 4 + ci x 3 + bi x 2 + ei x + f i‬‬ ‫תכנון הפולינום מבוסס על תכנון תנאי גבול מתאימים )כיוון ועקמומיות( בכל‬ ‫נקודת דרך‪ .‬כיוון המסלול בנקודה ) ‪ (xd i , yd i‬מסומן ע"י ‪ γ i‬ונקבע כך שהנורמל‬ ‫למסלול בנקודה זו חוצה את הזווית המתקבלת על ידי חיבור הנקודות‪:‬‬ ‫) ‪ (xd i , yd i ) , (xd i−1 , yd i−1‬ו‪ , (xd i+1 , yd i+1 ) -‬ראה איור ‪.A.2.2‬‬ ‫מתוך איור ‪ A.2.2‬ברור כי‪:‬‬ ‫‪ϕi = atan 2 ( yd i +1 − ydi ) , ( xd i +1 − xd i ) ‬‬ ‫)‪(A.2.2‬‬ ‫‪ϕi − ϕi −1 < π‬‬ ‫‪,‬‬ ‫‪ϕi +1 − ϕi < π‬‬ ‫‪,‬‬ ‫‪146‬‬ ‫‪ϕi −1 − ϕi‬‬ ‫‪2‬‬ ‫‪ϕ − ϕi‬‬ ‫‪γi = i +1‬‬ ‫‪2‬‬ ‫= ‪γi‬‬ ‫) ‪( xdi + 2 , ydi + 2‬‬ ‫‪ϕi +1‬‬ ‫‪γi‬‬ ‫) ‪( xdi +1 , ydi +1‬‬ ‫‪ϕi‬‬ ‫‪γi‬‬ ‫) ‪( xdi , ydi‬‬ ‫משיק למסלול‬ ‫בנקודה ‪i‬‬ ‫חוצה זווית‬ ‫ונורמל למסלול‬ ‫‪ϕi −1‬‬ ‫) ‪( xdi −1 , ydi −1‬‬ ‫איור ‪ – A.2.2‬כיוון הפולינום ) ‪ fi ( x‬בנקודות ) ‪ (xd i , yd i‬ו‪( xdi +1 , ydi +1 ) -‬‬ ‫הזוויות ‪ γ i‬ו‪ γi -‬במשוואה )‪ (A.2.2‬נתונות ביחס למערכת הצירים המוגדרת בנקודה‬ ‫) ‪) (xd i , yd i‬לצורך פישוט‪ ,‬תכנון המסלול הוגבל אך ורק לנקודות דרך המקיימות‬ ‫‪.( ϕi +1 − ϕi < π‬‬ ‫לצורך הגדרת הנגזרת השנייה ב‪ (xd i , yd i ) -‬נבנות שתי פונקציות עזר‪ .‬הפונקציה‬ ‫הראשונה היא פולינום ממעלה שלישית המחבר את הנקודה ) ‪ ( xdi −1 , ydi −1‬עם‬ ‫) ‪ (xd i , yd i‬ומקיימת את תנאי המקום והזווית בשתי הנקודות‪ ,‬הפונקציה השנייה גם‬ ‫כן פולינום ממעלה שלישית המחבר את הנקודה ) ‪ (xd i , yd i‬עם ) ‪( xdi +1 , ydi +1‬‬ ‫ומקיימת את תנאי המקום והזווית בנקודות אלה‪ .‬הנגזרת השנייה של המסלול‬ ‫בנקודה ) ‪ (xd i , yd i‬היא ממוצע הנגזרות השניות של שתי פונקציות העזר בנקודה‬ ‫) ‪. (xd i , yd i‬‬ ‫לסיכום‪ ,‬שישה תנאי הגבול שצריך לקיים הפולינום ) ‪ fi ( x‬במערכת צירים מקומית‬ ‫אשר ראשיתה נמצאת בנקודה ) ‪ (xd i , yd i‬נתונים ב‪.(A.2.3) -‬‬ ‫‪147‬‬ ‫‪fi ( 0 ) = 0‬‬ ‫‪fi ( l i ) = 0‬‬ ‫‪f i′ ( 0 ) = tan γ i‬‬ ‫‪f i′ ( l i ) = tan γi‬‬ ‫)‪(A.2.3‬‬ ‫‪tan γ i −1 + 2 tan γi −1 2 tan γ i + tan γi‬‬ ‫‪−‬‬ ‫‪l i −1‬‬ ‫‪li‬‬ ‫= ) ‪f i′′ ( 0‬‬ ‫‪tan γ i + 2 tan γi 2 tan γ i +1 + tan γi +1‬‬ ‫‪−‬‬ ‫‪li‬‬ ‫‪l i +1‬‬ ‫= ) ‪f i′′ ( l i‬‬ ‫כאשר ‪ γi , γ i‬נתונים ב‪ (A.2.2) -‬ו‪. l i = ( ydi +1 − ydi ) + ( xdi +1 − xdi ) -‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫הערה ‪ - 1‬חישוב הפולינום ) ‪) fi ( x‬בקטע מספר ‪ (i‬מתבסס על הנקודות‪( xdi − 2 , yi − 2 ) :‬‬ ‫) ‪ ( xdi + 2 , ydi + 2 ) , ( xdi +1 , ydi +1 ) , ( xdi , ydi ) , ( xdi −1 , ydi −1‬ו‪ , ( xdi + 3 , ydi + 3 ) -‬כלומר חישוב‬ ‫המסלול בכל נקודה לאורך המסלול הוא חישוב מקומי המבוסס על מספר קטן של‬ ‫נקודות‪ .‬עובדה זו מאפשרת תכנון ושינוי מסלול בזמן אמת הנובע מהוספה או הזזה‬ ‫של נקודות‪ ,‬לדוגמא בעקבות גילוי של מכשולים‪.‬‬ ‫הערה ‪ - 2‬תנאי הגבול של המסלול )המורכב מהנקודות ‪ ( ( xdi , ydi ) , i = 1K n‬הם‪:‬‬ ‫‪′ ( l n-1 ) = f n-1‬‬ ‫‪′′ ( l n-1 ) = 0‬‬ ‫‪f1′ ( 0 ) = f1′′( 0 ) = f n-1‬‬ ‫)‪(A.2.4‬‬ ‫הערה ‪ - 3‬רציפות בעקמומיות של המסלול )מסומנת ב‪ ( Ci ( x ) -‬מבוססת על‬ ‫הקשרים הבאים‪:‬‬ ‫) ‪fi′′( 0‬‬ ‫) ))‪(1 + ( f ′( 0‬‬ ‫‪2 32‬‬ ‫= ) ‪Ci ( 0‬‬ ‫‪i‬‬ ‫)‪(A.2.5‬‬ ‫‪,‬‬ ‫)‬ ‫‪2 32‬‬ ‫) ‪fi′′−1 ( l i −1‬‬ ‫) ) ‪( l i −1‬‬ ‫‪(1 + ( f ′‬‬ ‫= ) ‪Ci-1 ( l i −1‬‬ ‫‪i −1‬‬ ‫‪ tan γ i −1 + 2 tan γi −1 2 tan γ i + tan γi ‬‬ ‫‪−‬‬ ‫‪fi′′−1 ( l i −1 ) = f i′′( 0 ) = ‬‬ ‫‪‬‬ ‫‪l i −1‬‬ ‫‪li‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪ϕ‬‬ ‫‪−‬‬ ‫‪ϕ‬‬ ‫‪‬‬ ‫‪i −1 ‬‬ ‫) ) ‪= −f i′( 0 ) ⇒ ( f i′−1 ( l i −1 ) ) = ( f i′( 0‬‬ ‫‪fi′−1 ( l i −1 ) = tan γi −1 = tan  i‬‬ ‫‪‬‬ ‫‪2‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪148‬‬ ‫נספח ‪ – A.3‬תכנון מסלול רצוי המתאים לבקרים בגישה של מערכת‬ ‫קואורדינאטות מקומית‪.‬‬ ‫בסעיף זה מוצג תכנון מסלול רצוי העובר דרך סדרה של נקודות דרך רצויות‬ ‫ומתאים למימוש בקרים בגישה של מערכת קואורדינאטות מקומית‪.‬‬ ‫תכנון המסלול מבוסס על חיבור נקודות הדרך ‪ (way points) xd i , yd i‬עם עקומים‬ ‫פרמטריים ) ‪) f xi ( s ) , f yi ( s‬ראה איור ‪.(A.3.1‬‬ ‫)‪fx3 ( s) , f y3 ( s‬‬ ‫‪xd 4 , yd 4‬‬ ‫‪xd1 , yd1‬‬ ‫‪s:3→4‬‬ ‫) ‪f x1 ( s ) , f y1 ( s‬‬ ‫‪xd 3 , yd 3‬‬ ‫‪s :1 → 2‬‬ ‫) ‪f x0 ( s ) , f y0 ( s‬‬ ‫) ‪f x2 ( s ) , f y2 ( s‬‬ ‫‪s:2→3‬‬ ‫‪Y‬‬ ‫‪s : 0 →1‬‬ ‫‪xd 2 , yd 2‬‬ ‫‪xd 0 , yd 0‬‬ ‫‪X‬‬ ‫איור ‪ – A.3.1‬מסלול המבוסס על עקומים פרמטריים‬ ‫העקומים ) ‪ f xi ( s ) , f yi ( s‬מתוכננים כך שתשמר רציפות במקום‪ ,‬בכיוון‪ ,‬בעקמומיות‬ ‫ובקצב שינוי העקמומיות‪ .‬כל עקום המחבר שתי נקודות דרך צריך לקיים שמונה‬ ‫תנאי גבול הנתונים בטבלה הבאה‪:‬‬ ‫בנקודה ‪xd i , yd i‬‬ ‫בנקודה ‪xd i +1 , yd i +1‬‬ ‫מקום‪:‬‬ ‫‪xd i , yd i‬‬ ‫‪xd i +1 , yd i +1‬‬ ‫כיוון‪:‬‬ ‫‪γi‬‬ ‫‪γ i+1‬‬ ‫עקמומיות‪:‬‬ ‫‪Ci‬‬ ‫‪C i +1‬‬ ‫קצב שינוי העקמומיות‪:‬‬ ‫‪dC i‬‬ ‫‪ds‬‬ ‫‪dC i+1‬‬ ‫‪ds‬‬ ‫טבלה ‪ – A.3.1‬תנאי גבול‬ ‫בהמשך נשתמש בסימנים הבאים‪ x ( s ) = f x ( s ) :‬ו‪y ( s ) = f y ( s ) -‬‬ ‫‪149‬‬ ‫תכנון כל עקום פרמטרי המחבר שתי נקודות דרך עוקבות מבוסס על פולינומים‬ ‫מדרגה ‪ 7‬המוגדרים באופן הבא‪:‬‬ ‫‪x i (s ) = a x s 7 + b x s 6 + c x s 5 + d x s 4 + e x s 3 + f x s 2 + g x s + h x‬‬ ‫‪y i (s ) = a y s 7 + b y s 6 + c y s 5 + d y s 4 + e y s 3 + f y s 2 + g y s + h y‬‬ ‫)‪(A.3.1‬‬ ‫‪s : i → i +1‬‬ ‫כאשר הפרמטר ‪ s‬אינו בהכרח אורך העקום )אורך העקום האמיתי יסומן בהמשך‬ ‫באות ‪.(S‬‬ ‫כיוון העקום הפרמטרי בנקודה ‪ s‬מוגדר ע"י‪:‬‬ ‫‪ dx dy ‬‬ ‫‪γ ( s ) = atan 2  ,  = atan 2 ( k cos γ , k sin γ ) , k ≠ 0‬‬ ‫‪ ds ds ‬‬ ‫)‪(A.3.2‬‬ ‫כאשר ‪ k‬הינו פרמטר אשר לא משפיע על כיוון העקום בנקודה ‪ s‬ויקבע בהמשך‪.‬‬ ‫מתוך )‪ ,(A.3.2‬כדי לקיים את תנאי הגבול השני בטבלה ‪) A.3.1‬כיוון( דרוש‪:‬‬ ‫‪dy‬‬ ‫‪= k sin γ‬‬ ‫‪ds‬‬ ‫)‪(A.3.3‬‬ ‫‪dx‬‬ ‫‪= k cos γ‬‬ ‫‪ds‬‬ ‫‪,‬‬ ‫ניתן להראות כי הפרמטר ‪ k‬הוא היחס בין אורך העקום ‪ S‬והפרמטר ‪.s‬‬ ‫)‪(A.3.4‬‬ ‫‪=k‬‬ ‫‪2‬‬ ‫) ‪( k cos γi ) + ( k sin γi‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪dS‬‬ ‫‪ dx   dy ‬‬ ‫= ‪=   + ‬‬ ‫‪ds‬‬ ‫‪ ds   ds ‬‬ ‫⇒‬ ‫‪dS = dx 2 + dy 2‬‬ ‫עקמומיות העקום הפרמטרי בנקודה ‪ s‬מוגדרת באופן הבא‪:‬‬ ‫)‪(A.3.5‬‬ ‫‪ d 2 y   dx   d 2 x   dy ‬‬ ‫‪ 2   −  2  ‬‬ ‫‪ 1‬‬ ‫‪ds   ds   ds   ds   d 2 y‬‬ ‫‪d2x‬‬ ‫‪C (s) = ‬‬ ‫‪cos‬‬ ‫‪sin γ  2‬‬ ‫=‬ ‫‪γ‬‬ ‫‪−‬‬ ‫‪ 2‬‬ ‫‪3‬‬ ‫‪2‬‬ ‫‪ds‬‬ ‫‪ ds‬‬ ‫‪k‬‬ ‫‪  dx 2  dy  2  2‬‬ ‫‪  +   ‬‬ ‫‪  ds   ds  ‬‬ ‫‪‬‬ ‫‪‬‬ ‫כאשר ב‪ (A.3.5) -‬הצבנו את )‪ .(A.3.3‬ניתן לקיים את התנאי השני בטבלה ‪A.3.1‬‬ ‫)עקמומיות( ע"י קביעת הנגזרות השניות באופן הבא‪:‬‬ ‫)‪(A.3.6‬‬ ‫‪d2x‬‬ ‫‪= −Ck 2 sin γ‬‬ ‫‪ds 2‬‬ ‫‪150‬‬ ‫‪,‬‬ ‫‪d2 y‬‬ ‫‪= Ck 2 cos γ‬‬ ‫‪ds 2‬‬ ‫קצב שינוי העקמומיות מתקבל מתוך גזירה של )‪(A.3.5‬‬ ‫‪ 1 ‬‬ ‫‪dC  dC   dS  d   d 2 y‬‬ ‫‪d2x‬‬ ‫‪=‬‬ ‫‪=   2 cos γ − 2 sin γ  3 ‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪dS  ds   ds  ds   ds‬‬ ‫‪ds‬‬ ‫‪k ‬‬ ‫)‪(A.3.7‬‬ ‫‪3‬‬ ‫‪3‬‬ ‫‪d3 y‬‬ ‫‪d2 y‬‬ ‫‪dγ d 3x‬‬ ‫‪d2x‬‬ ‫‪dγ  d y  cos γ −  d x  sin γ‬‬ ‫‪γ‬‬ ‫‪−‬‬ ‫‪γ‬‬ ‫‪−‬‬ ‫‪γ‬‬ ‫‪−‬‬ ‫‪γ‬‬ ‫‪cos‬‬ ‫‪sin‬‬ ‫‪sin‬‬ ‫‪cos‬‬ ‫‪3‬‬ ‫‪3‬‬ ‫‪3‬‬ ‫‪ ds ‬‬ ‫‪ds ds3‬‬ ‫‪ds =  ds ‬‬ ‫‪ds 2‬‬ ‫‪ds2‬‬ ‫‪= ds‬‬ ‫‪3‬‬ ‫‪3‬‬ ‫‪k‬‬ ‫‪k‬‬ ‫כאשר ב‪ (A.3.7) -‬הצבנו את )‪.(A.3.6‬‬ ‫דרך אחת לקיום תנאי הגבול הרביעי בטבלה ‪) A.3.1‬קצב שינוי העקמומיות( הוא‬ ‫קביעת הנגזרות )מסדר שלישי( של העקום באופן הבא‪:‬‬ ‫)‪(A.3.8‬‬ ‫‪ d3x ‬‬ ‫‪dC 3‬‬ ‫‪k sin γ‬‬ ‫‪ 3 =−‬‬ ‫‪ds‬‬ ‫‪ ds ‬‬ ‫‪,‬‬ ‫‪ d 3 y  dC 3‬‬ ‫‪k cos γ‬‬ ‫=‪ 3 ‬‬ ‫‪ ds  ds‬‬ ‫לצורך פשטות התכנון נדרוש שקצב שינוי העקמומיות בנקודות הקצה של העקום‬ ‫יהיה אפס‪ .‬מכאן דרוש‬ ‫‪ d3y   d3x ‬‬ ‫‪‬‬ ‫‪ ‬‬ ‫‪‬‬ ‫‪ ds 3  =  ds 3  = 0‬‬ ‫‪‬‬ ‫‪ ‬‬ ‫‪‬‬ ‫)‪(A.3.9‬‬ ‫הערה‪ :‬את )‪ (A.3.6) ,(A.3.3‬ו‪ (A.3.8) -‬יש לרשום עבור שני הצדדים של הפולינום‪.‬‬ ‫כיוון העקום הפרמטרי בנקודות הדרך זהה לכיוון העקום כפי שנקבע בנספח ‪A.2‬‬ ‫)מסלול לבקר בגישה של מערכת קואורדינאטות קבועה במרחב(‪ ,‬כלומר כך‬ ‫שהנורמל למסלול בנקודה ) ‪ (xd i , yd i‬חוצה את הזווית המתקבלת על ידי חיבור‬ ‫הנקודות‪ (xd i , yd i ) , (xd i−1 , yd i−1 ) :‬ו‪ , (xd i+1 , yd i+1 ) -‬ראה איור ‪.A.3.2‬‬ ‫) ‪(xd i+1 , yd i+1‬‬ ‫משיק‬ ‫‪γi‬‬ ‫‪ϕi‬‬ ‫נורמל‬ ‫) ‪(xd i , yd i‬‬ ‫‪ϕ i−1‬‬ ‫) ‪(xd i−1 , yd i−1‬‬ ‫‪X‬‬ ‫איור ‪ – A.3.2‬כיוון המסלול בנקודה ) ‪(xd i , yd i‬‬ ‫‪151‬‬ ‫‪Y‬‬ ‫מתוך האיור ברור כי‪:‬‬ ‫‪ϕi −1 = atan 2 ( yd i − ydi −1 ) , ( xd i − xd i −1 ) ‬‬ ‫‪ϕi = atan 2 ( yd i +1 − yd i ) , ( xd i +1 − xd i ) ‬‬ ‫)‪(A.3.10‬‬ ‫‪( ϕi + ϕi −1 ) 2‬‬ ‫‪ϕi − ϕi −1 ≤ π‬‬ ‫‪,‬‬ ‫‪ϕi − ϕi −1 > π‬‬ ‫‪2+π ,‬‬ ‫) ‪( ϕi + ϕi −1‬‬ ‫‪‬‬ ‫‪γi = ‬‬ ‫‪‬‬ ‫הערה‪ :‬כאשר ‪ ϕi − ϕ i−1 > π‬כיוון הסיבוב הוא עם כיוון השעון )הפוך מהנתון‬ ‫באיור(‪.‬‬ ‫לצורך קביעת העקמומיות בנקודה ) ‪ (xd i , yd i‬מוגדר עקום עזר מהצורה הבאה‪:‬‬ ‫‪x i ( s ) = a x s5 + b x s 4 + c x s3 + d x s 2 + e x s + f x‬‬ ‫‪yi ( s ) = a y s5 + b ys 4 + c ys 3 + d y s 2 + e ys + f y‬‬ ‫)‪(A.3.11‬‬ ‫‪s = −1 → 1‬‬ ‫עקום זה עובר דרך הנקודות‪ (xd i , yd i ) , (xd i−1 , yd i−1 ) :‬ו‪) (xd i+1 , yd i+1 ) -‬ראה איור‬ ‫‪ (A.3.3‬ומקיים את תנאי הכיוון ‪ γ i , γ i−1‬ו‪. γ i+1 -‬‬ ‫‪s =1‬‬ ‫‪ϕi−1‬‬ ‫‪s=0‬‬ ‫‪ϕi‬‬ ‫) ‪(xd i , yd i‬‬ ‫) ‪(xd i−1 , yd i−1‬‬ ‫‪ϕi−2‬‬ ‫‪Y‬‬ ‫‪s = −1‬‬ ‫‪ϕi+1‬‬ ‫) ‪(xd i+2 , yd i+2‬‬ ‫) ‪(xd i+1 , yd i+1‬‬ ‫) ‪(xd i−2 , yd i−2‬‬ ‫‪X‬‬ ‫איור ‪ – A.3.3‬עקום עזר לקביעת עקמומיות‬ ‫עקום העזר משמש אך רק לקביעת העקמומיות של המסלול ) ‪ x (s ), y(s‬בנקודה‬ ‫) ‪ ( xdi , ydi‬ומתוכנן לפי תנאי הגבול הנתונים בטבלה ‪.A.3.2‬‬ ‫בנקודה ‪xd i , yd i‬‬ ‫בנקודה ‪xd i , yd i‬‬ ‫בנקודה ‪xd i +1 , yd i +1‬‬ ‫מקום‪:‬‬ ‫‪xd i −1 , yd i −1‬‬ ‫‪xd i , yd i‬‬ ‫‪xd i +1 , yd i +1‬‬ ‫כיוון‪:‬‬ ‫‪γ i−1‬‬ ‫‪γi‬‬ ‫‪γ i+1‬‬ ‫טבלה ‪ - A.3.2‬תנאי גבול של עקום העזר‬ ‫‪152‬‬ ‫מכיוון שעבור עקום העזר הפרמטר ‪ s‬שווה אפס בנקודה ) ‪ , ( xdi , ydi‬הביטוי עבור‬ ‫העקמומיות של המסלול בנקודה זו נתון ע"י‪:‬‬ ‫)‪(A.3.12‬‬ ‫) ‪ d 2 y (s‬‬ ‫‪‬‬ ‫) ‪ d 2 x (s‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪‬‬ ‫‪ sin γ i‬‬ ‫‪γ‬‬ ‫‪−‬‬ ‫‪cos‬‬ ‫‪i‬‬ ‫‪ ds 2‬‬ ‫‪‬‬ ‫‪ ds 2‬‬ ‫‪‬‬ ‫‪s=0 ‬‬ ‫‪s=0 ‬‬ ‫‪‬‬ ‫‪=‬‬ ‫‪ki2‬‬ ‫‪2d y cos γ i − 2d x sin γ i‬‬ ‫‪ki2‬‬ ‫‪s=0‬‬ ‫) ‪Ci = C ( s‬‬ ‫=‬ ‫כאשר ‪ d x‬ו‪ d y -‬הם מקדמים של פולינום העזר )‪ γ i ,(A.3.11‬נתון ב‪ (A.3.10) -‬ו‪k i -‬‬ ‫יקבע בהמשך‪.‬‬ ‫תכנון הקבועים ‪ k i‬ו‪: ki -‬‬ ‫הראנו ב‪ (A.3.4) -‬כי המשתנה ) ‪ k ( s‬הינו היחס בין האורך האמיתי של העקום ‪ S‬ובין‬ ‫‪dS‬‬ ‫הפרמטר ‪) s‬המשתנה מ‪ i -‬עד ‪ i + 1‬בכל קטע(‪ ,‬כלומר ) ‪= k ( s‬‬ ‫‪ds‬‬ ‫‪ .‬ככל ש‪ k -‬יהיה‬ ‫יותר קטן העקום יהיה יותר קצר ולהפך כאשר ‪ k‬גדול‪ .‬מכיוון שבקצוות קיימים‬ ‫אילוצי כיוון‪ ,‬שימוש בערכים גדולים מדי או קטנים מדי של המשתנה ‪ k‬יגרמו‬ ‫לקבלת ערכים גבוהים של עקמומיות באזורים שונים של העקום‪ .‬כדי לקבל עקום‬ ‫"מתון" ונוח לנסיעה )בעל עקמומיות המשתנה לאט וללא ערכי עקמומיות גבוהים(‬ ‫ברור כי יש לבחור את הפרמטר ‪ k‬כך שיהיה יחסי למרחק האמיתי בין נקודות‬ ‫הדרך‪ .‬נמצא כי בחירה מתאימה של הפרמטרים ‪ k i‬היא‪:‬‬ ‫)‪(A.3.13‬‬ ‫‪0.475‬‬ ‫‪2‬‬ ‫‪2‬‬ ‫‪k i = ( xd i +1 − xd i ) + ( yd i +1 − yd i ) ‬‬ ‫‪‬‬ ‫‪‬‬ ‫ועבור עקום העזר )‪(A.3.11‬‬ ‫‪k i −1 = k i −1‬‬ ‫)‪(A.3.14‬‬ ‫) ‪k i = 0.5 ( k i −1 + k i‬‬ ‫‪k i +1 = k i‬‬ ‫‪153‬‬ ‫מקורות‬ [Aguilar et al. 1988] Aguilar L. E., Soueres M. P., Courdesses M. and Fleury S., “Robust path-following control with exponential stability for mobile robots”, Proceedings of the 1998 IEEE Int. Conference on Robotics & Automation, Leuven, Belguim, May 1988. [Ailon et al. 2001] Ailon A., Berman N. and Arogeti S., “On controllability and trajectory following of a kinematic car”, Proceedings of the IFAC Symposium on System & Control, Prague, Czech, August 2001. [Ailon et al. 2005] Ailon A., Berman N. and Arogeti S., “On controllability and trajectory tracking of a kinematic vehicle model”, ", Automatica, Vol. 41, pp. 889-896, 2005 [Balluchi et al. 1996] Balluchi A., Bicchi A., Balestrino A. and Casalino G., "Path tracking control of Dubin's car", In Proc. 1996 IEEE International Conference on Robotics and Automation, Vol. 4, Minneapolis, Minesota, USA, pp. 3123--3128, 1996. [Bonnifait et al. 2001] Bonnifait P., Bouruon P., Crubille P. and Dominique M., "Data Fusion of Four ABS Sensors and GPS for an Enhanced Localization of Car-like Vehicles", Proceedings of the 2001 IEEE International Conference on Robotics and Automation, Seoul, May, 2001. [Boyd et al. 1994] Boyd S., Ghaoui L. E., Feron E. and Balakrishnan V., "Linear Matrix Inequalities in System and Control Theory", SIAM Studies in Applied Mathematics, 1994. [Brockett 1983] Brockett R. W., "Asymptotic stability and feedback stabilization", in "Differential Geometric Control Theory", R. W. Brockett, R. S. Millman, and H. J. Sussmann,. pp. 437-442, Boston, MA: Birkhauser, 1983. [Chwa 2004] Chwa D., " Sliding-Mode Tracking Control of Nonholonomic Wheeled Mobile Robots in Polar Coordinates", IEEE transaction on Control Systems Technology, Vol. 12, No. 4, July 2004. 154 [Consolini et al. 2001] Consolini L., Piazzi A., and Tosques M., "Motion planning for steering car-like vehicles", Proceedings of the European Control Conference, pp. 1834-1839, 2001. [De Luca et al. 1998] De Luca A., Oriolo G. and Samons C., “Feedback control of a nonholonomic car-like robot”, Laboratories d’Analye d’Architecture des Systemes, LAAS report 97438, 1998 [DMATM 1989] Defense Mapping Agency Technical Manual (DMATM) 8358.2, "The Universal Grids: Universal Transverse Mercator (UTM) and Universal Polar Stereographic (UPS)", September 1989. [Dickmans & Dickmans E. D. and Mysliwetz B. D., “Recursive 3-D road and Mysliwetz 1992] relative ego-state estimation”, IEEE Transaction on PAMI, 14(2): 199-213, February 1992. [Dixon et al. 2000] Dixon W. E., Dawson D. M. and Zergeroglu E., "Tracking and regulation control of a mobile robot system with kinematics Disturbances: A variable structure-like approach", journal of dynamic system, measurement, and control, Vol. 122, pp. 616623, December 2000. [Dixon et al. 2001] Dixon W. E., Dawson D. M., Zergeroglu E. and Behal A., “Nonlinear control of wheeled mobile robot”, Lecture Notes in Control and Information Sciences 262, Springer, UK, 2001. [Dubins 1957] Dubins L. E., "On curves of minimal length with a constraint on average curvature and with prescribed initial and terminal position and tangents", American Journal of Mathematics", Vol. 79, pp. 497-516, 1957. [Frezza et al. 1998] Frezza R., Soatto S. and Picci G., “A Lagrangian formulation of nonholonomic path following”. In “The Confluence of Vision and Control”, S. Morse et al. (eds), Springer Verlag. 1998. [Garnier & Fraichard Garnier P. and Fraichard T., "A Fuzzy Motion Controller for a 1996] Car-Like Vehicle", International Conference on Intelligent Robots and Systems, Osaka, Japan, Nov. 1996. [Gillespie 1992] Gillespie T. D., "Fundamentals of Vehicle Dynamics", SAE International, May, 1992. 155 [Gusev et al. 1998] Gusev S. V., Makarov I. A., Paromtchik I. E., Yakubovich V. A. and Laugier C., "Adaptive motion control of nonholonomic Vehicle", Proceedings of the 1998 IEEE International Conference on Robotics & Automation, Leuven, Belgium, May 1998. [Hajjaji & Bentalba Hajjaji A. E. and Bentalba S., "Fuzzy Path tracking Control for 2003] Automatic Steering of Vehicles", Robotics and Automation Systems, Vol. 43, pp. 203-213, 2003. [Indiveri 1999] Indiveri G., "Kinematic Time-invariant Control of a 2DNonholonomic Vehicle", IEEE Conference on Decision and Control, Phoenix, Dec. 1999. [Isidori et al. 1995] Isidori A., "Nonlinear Control Systems", 3rd edition SpringerVerlag, June, 1995. [Jiang & Nijmeijer Jiang Z. P. and Nijmeijer H., "Tracking control of mobile 1997] robots: a case study in backstepping", Automatica, Vol. 33, No. 7, pp. 1393-1399, 1997 [Khalil 1996] Khalil H. K., “Nonlinear systems”, Second Edition, Prentice Hall, New Jersey, 1996 [Kim & Oh 1999] Kim D. H. and Oh J. H., "Tracking control of a two-wheeled mobile robot using input-output linearization", Control Engineering Practice 7, pp 369-373, 1999. [Kosecka et al. 1998] Kosecka J., Blasi R., Taylor C. J. and Malik J., “A comparative study of vision-based lateral control strategies for autonomous highway driving”, Proceedings of IEEE Int. Conf. on Robotics and Automation, Leuven, Belgium, May 1998. [Laiou & Astolfi Laiou M. C., Astolfi A., "Discontinuous Contor of High-Order 1999] Generlized Chained Systems", System & Control Letters, Vol. 37, pp. 309-322, 1999. [Laugier et al. 1998] Laugier C., Fraichard T., Garnier P., Paromtchik I. and Scheuer A., “Sensor-based control architecture for a car-like vehicle”, Institute National de Recherche en Informatique Automatique, INRIA N o 3552, October 1998. 156 en [Levant 2001] Levant A., “Higher order sliding modes and arbitrary-order exact robust differentiation”, European Control Conference, Porto, Portugal, 2001. [Lu et al. 2000] Lu J., Sakhavat S., Xie M. and Laugier C., “Sliding mode control for nonholonomic mobile robot”. In Proc. of the Conf. on Control , Automation, Robotics and Vision, Singapore, Dec. 2000. [Ma 2000] Ma Y., “A differential geometric approach to computer vision and its applications in control”, Ph.D. thesis, Electrical Engineering and Computer Sciences, University of California at Berkeley, 2000. [Ma et al. 1999] Ma Y., Kosecka J. and Sastry S., “Vision Guided Navigation for a nonholonomic mobile robot”, IEEE Trans. on Robotics and Automation, Vol. 15, No. 3, pp. 521-537, June 1999. [Majjar & Bonnifait Majjar M.E.E. and Bonnifait P., "A Roadmap Matching Method 2003] for Precise Vehicle Localization using Belief Theory and Kalman Filtering", Proceedings of the 11th International Conference on Advanced Robotics, Coimbra, Portugal, 2003. [Mammar et al. 2001] Mammar S., Koenig D. and Nouvelier L., "Combination of feedforward and feedback H∞ control for speed scheduled vehicle automatic steering", Proceedings of the American control conference, Arlington, VA June 2001. [Samson 1993] Samson C., "Time-Varying Feedback Stabilization of Car-like Wheeled Mobile Robot", The International Journal of Robotics Research, Vol. 12, No. 1, Feb., 1993. [Samson 1995] Samson C., "Control of Chained Systems Application to Path Following and Time-Varying Point-Stabilization of Mobile Robots", IEEE Trans. on Automatic Control Vol. 40, pp. 802819, May 1995. [van der Schaft 2000] van der Schaft A., "L2 – Gain and Passivity Techniques in Nonlinear Control", Springer-Verlag, London, 2000. 157 [Scherer & Weiland Scherer C. and Weiland S., Lecture Notes DISC Course on 1999] Linear Matrix Inequalities in Control, page 57, Version 2.0, April, 1999. [Sørdalen & Egeland Sørdalen O. J. and Egeland O., "Exponential Stabilization of 1995] Nonholonomic Chaind Systems", IEEE Trans. on Automatic Control, Vol. 40, No. 1 pp. 35 - 49, Jan. 1995. [Stengel 1994] Stengel R. F., "Optimal Control and Estimation", pp. 367-388 Dover Publication Inc., New York, 1994. [Tayebi et al. 1997] Tayebi A., Tadjine M., and Rachid A., "Quasi- Continuous Output Feedback Control for Nonholonomic Systems in Chained Form". Proc. of 4th ECC, Brussels, Belgium, July 1997. [Tayebi et al. 2001] Tayebi, A., Tadjine M., Rachid A., "Invariant Manifold Approach for the Stabilization of Nonholonomic Chained Systems: Application to Mobile Robots", Nonlinear Dynamics, Vol. 24, pp. 167-181, 2001. [Tilbury et al. 1995] Tilbury D., Murry R., and Sastry S., "Trajectory generation for the n-trailer problem using Goursat Normal Form", IEEE Trans. on Automatic Control Vol. 40, pp. 802-819, May 1995. [Tomizuka & Hingwe Tomizuka M and Hingwe P., "Vehicle Lateral Control for 1998] Automated Highway Systems", Proceedings of the 4th International Symposium on Advanced Vehicle Control , Nagoya Congress Center, Nagoya, Japan, pp 25-32, September 14-18,1998 [Wang et al. 2004] Wang Z. P., Ge S. S. and Lee T. H., "Adaptive Neural Control of a Wheeled Mobile Robot Violating the Pure Nonholonomic Constraint", The 43rd IEEE Conference on Decision and Control, 2004. [Wong 2001] Wong J. Y., "Theory of Ground Vehicles", 3rd Edition, John Wiley & Sons Inc., March, 2001. 158 [Xu & Huo 2000] Xu W. L. and Huo W., “Variable structure exponential stabilization of chained systems based on the extended nonholonomic integrator”, System & Control Letters (41), pp. 225-235, 2000. [Yang et al. 1998] Yang J. M., Choi I. H and Kim J. H., "Sliding Modes Control of Nonholonomic Wheeled Mobile Robot for Trajectory Tracking", Proceedings of the 1998 IEEE International Conference on Robotics and Automation, Leuven, Belgium, May 1998. [Zhang et al. 2001] Zhang J. R., Xu S. J. and Rachid A., "Sliding Mode Lateral Motion Control for Automatic Steering of Vehicles", Proceedings of the European Control Conference, 2001. 159 Summary This work deals with design and realization of control systems for autonomous vehicles. The objective of the various algorithms that are conceived in this work is the achievement of an efficient path-following, i.e. tracking a predetermined geometric trajectory. To a lesser extent, trajectory-following problems are also considered, where in this case, in addition to a path-following, the vehicle is bound to observe a "time-table" along the trajectory. The approach taken in this work is based on a design of a general controller for an n-dimensional system while its kinematical model may be described by what is called chained-model, or chained-system. The controller introduced in this work is nonlinear and its advantages over other control laws designated for solving the path-following problem via the utilization of the chained-model, is in that it is smooth (thus, differs from other controllers of a sliding-mode type), and in its capability ("having the choice" of employing an appropriate nonlinear function) of avoiding a production of large control signals while the vehicle is far away from the desired path (contrary to linear controllers' performance). In addition to the development of this general controller, it is demonstrated how one can utilize this controller in the solution of the path-following problem, for large variety of scenarios which stem from the use of different types of vehicles, from various definitions of errors regarding the vehicle position relative to the desired path and from the error model development relative to coordinate systems, either local or fixed in space. Most of this work is aimed at design and realization of control systems in a four wheels vehicle equipped with a rear drive and a front steering system. Despite this, the general approach taken in this work, which is based on the utilization of chained-model, is applicable, as well, for different types of vehicles, for example, we introduce in this work a controller design methodology based on similar principles also for a differential drive vehicle (this type of vehicle possesses two driving wheels while the vehicle steering is performed by means of applying a differential angular velocity to the driving wheels). Additional classification (independent of the vehicle type) of the controllers introduced in this work is related to the coordinate system with respect to which the vehicle's state and the desired path are to be measured. In the first case we consider controllers that are based on the use of fixed in space (inertial) coordinate system. Such controllers are applicable while the vehicle's state is measured by sensors like GPS, DGPS, INS and a magnetic compass. The second case is represented by the use of local coordinate system which is a fixed in the vehicle's frame or attached to the path. Sensors that are commonly utilized in this case are video camera and laser scanner which are mounted on the vehicle, and they measure the vehicle's state relative to the desired path. Measurements of the vehicle's state, relative to inertial coordinate system, is redundant in this case. The representation of a vehicle's motion in terms of a chained model is achieved by a utilization of a state transformation and by an application of an appropriate control signal (feedback linearization). Two such transformations are known in the literature, one is based on space-fixed coordinate system, and has a singular point which becomes critical once the vehicle is oriented in perpendicular to the x-axis. The second is based on a coordinate system which is attached to the desired path. Aside from these transformations which are utilized, in this work, in representing and realizing the general controller, additional novel transformations, are introduced. In the case where the coordinate system is fixed in space, an additional transformation is introduced which exhibits a singular state related to the vehicle orientation relative to the desired path. One of the advantages of this new transformation stems from the fact that the singular state is independent of the coordinate system. Yet other novel transformations which are introduced here in connection to a chained model are: one which is based on a utilization of a polar coordinate system fixed in space, and the other one is characterized by coordinate system which is attached to the vehicle. All these transformations include singular states, and as such they allow local stability only. Some of the controllers that are to be introduced in the sequel are accompanied by sufficient conditions, developed in this thesis for local stability, which are related to the vehicle initial conditions, to the desired path attributes, and the exponential stability properties of the chained model together with the general controller as applied to this system. An additional novel achievement of this work, which sets up a common ground for all of the mentioned above transformations (resulting in a chained model), is based on a design of a very general invariant controller from which one can deduce all the other controllers as special cases. The kinematical models of the vehicles considered in this work assume "roll with no slip". According to this hypothesis the lateral velocity (orthogonal to the wheel's direction) of each wheel is zero. In fact, a slip is always present whenever a vehicle moves along a nonstraight line, while the slip angle magnitude is proportional to the lateral force which is exerted on the wheel. These slip angles may be significant, in particular while a vehicle performs sharp maneuvers in a high speed. In low speed regimes the hypothesis of "roll b with no slip" is valid. This is supported by the vehicle motion performances as they reflect in the experiment data and simulation results obtained by application of the controllers, developed in this thesis, in a dynamical model. When high speed maneuvers are required, which results in large slip angles, the assumption of roll with no slip is not valid anymore and the design requires a utilization of a model which accounts for slip angles. The common model in this case is a dynamical model. As this dynamical model is complicated and nonlinear, most controllers that have been developed so far on the basis of dynamical models assume constant speed motion and small steering angles. These assumptions have facilitated a linearization of the underlying model and allow the use of linear methods in the design. The assumption regarding a vehicle motion along highways with no sharp turns, is largely valid and results in fairly good autonomous motion performances. The algorithms introduced in this work are designated to perform in vehicles that operate in an outdoor environment (not necessarily highways of the type mentioned above), so the assumptions regarding constant speed and small steering angles are invalid. This work introduces a novel approach in the development of the kinematical model which accounts for roll with slip. This development is based on a linearization around the desired path, without assuming small steering angles or constant speed. The resulting model consists of two parts. The first one is identical to a chained model (under no slip hypothesis), while the second part includes all the slip effects and is viewed in this work as an unknown perturbation (entitled disturbance) of the first part. The controller design objective now is a minimization of the disturbances' effects on the errors of the vehicle state relative to the desired path. The design methodology is based on a minimization of what is called "peak-to-peak" criterion, together with an application of the Linear Matrix Inequalities (LMI) methods. Simulations demonstrate that this methodology is superior to all of the other methodologies investigated in this thesis when applied to a dynamical model which accounts for slip. A main objective of this work is the development of control algorithms that are realizable in real vehicles control systems. An experimental system has been built and used for the purpose of evaluating this applicability. This experimental system consists of a four-wheel electrical car, and other means for facilitating the algorithms realization which include: desired path design, coordinate system change in the course of operation and sensors' data processing. c In general the kinematical and dynamical models of a four-wheel vehicle are nonlinear, so that filtering of noisy measurements calls for the utilization of what is called Extended Kalman Filter (EKF). A different approach is also introduced in this work, regarding the problem of filtering, where a linear (time-varying) setup is considered so that linear filtering theory may be employed. Applications of the controllers which have been introduced in this work require the desired path to be available to the controller in an analytical form. This raises difficulties regarding implementations. In order to circumvent these difficulties, a method for representing a desired path by means of "way points" which are determined via an available data of the pertinent aerial scene. In this connection, algorithms for a path design in real time developed. These algorithms guarantee desired smoothness of the designed path, which passes through a sequence of desired way points, possesses certain geometrical properties which allow an efficient implementation of the control system and a smooth driving of the autonomous vehicle. The theoretical results of this thesis were tested and demonstrated partly via simulations and partly via the implementation of the control systems in the electrical car and performing outdoor experiments which demonstrated the overall system's performance, and the efficiency of methodology which was developed in this work. Key words: autonomous vehicles, mobile robots, chained systems, path following, trajectory tracking, nonlinear control, path planning. d Autonomous Vehicles control Thesis submitted in partial fulfillment of the requirements for the degree of “DOCTOR OF PHILOSOPHY” by Shai Arogeti Submitted to the Senate of Ben-Gurion University of the Negev December 2005 Beer-Sheva Autonomous Vehicles control Thesis submitted in partial fulfillment of the requirements for the degree of “DOCTOR OF PHILOSOPHY” by Shai Arogeti Submitted to the Senate of Ben-Gurion University of the Negev Approved by the advisor: ________________ Approved by the Dean of the Kreitman School of Advanced Graduate Studies: ________________ December 2005 Beer-Sheva This work was carried out under the supervision of: Dr. Nadav Berman . In the Department of Mechanical Engineering Faculty of Engineering .

ArogetiShai

Related documents

Products

Support

ArogetiShai

Related documents

Add this document to collection(s)

Add this document to saved

Suggest us how to improve StudyLib