1

מה נראה היום לישראל טכנולוגי מכון - הטכניון

TECHNION - ISRAEL INSTITUTE OF TECHNOLOGY

חשמל להנדסת הפקולטהורובוטיקה לבקרה המעבדה

בעזרת IMU ניווט רובוט מבוססMatlab

מגישיםאור רוזנבוים יותם מיכאלי

מנחהד"ר גבי דוידוב

סמסטר: חורף/אביבשנה: תשס"ז

2

מה נראה היום

סקירת הבעיה•

פתרונות אפשריים•

הפתרון הנבחר•

תוצאות•

סיכום•

מסקנות•

3

סקירת הבעיה

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

מהירה ואוטונומית.

בלבד לא מספקות.GPSמדידות •

זול ישנה שגיאת מדידה גדולה IMUלמדיד •בנוסף לשגיאת היסט.

מאופיין IMU, GPSהמודל עבור ניווט משולב •באי-ליניאריות.

4

פתרונות אפשריים

– אינטרפולציהGPSשימוש רק ב-•

יותר טובIMUהקטנת הרעש – •

EKF, UKF, Monte Carloסינון הנתונים – •

לא לצאת מהבית•

5

הפתרון הנבחר

IMU+GPSשילוב מדידות •

6

הפתרון הנבחרUKFקופסא שחורה : סינון ע"י •

1

1 1|

1 1|

ˆk k k k

k k

k X Y Y Y

k k k k k k

xx xx Tk k k k Y Y k

K P P

x x K y y

P P K P K

0..21|

0..2

1| 1|

ˆ

ˆ

ˆ ˆ

i L ik k k

i Lk i k

Txy i ik i k k k k k k

Tyy i ik i k k k k

h

y W

P W x y

P W y y R

0..21|

0..21| 1|

1| 1| 1| 1| 1|

i L ik k k

i Lk k i k k

Txx i ik k i k k k k k k k k

f

x W

P W x x Q

ומדידתםSPייצור ה-חישוב סטטיסטיקת המדידה

המשוערכת

וקידומם ע"פ מודלSPייצור ה-חישוב סטטיסטיקת המצב המשוערך

עדכון המצב ע"י משוב משגיאת המדידה

UPDATE

PROPAGATE

PREDICT

7

הפתרון

הנבחרUKFסינון ע"י •

Zoom in

1X k kPropogation by model

+IMU measurments

1..2

,

i L

kcalculae

from X Pxx

1ik k

statei

state

IMU measurments

Y

W weightsi

ˆ , ,

,

yy xyi

Calculate y P Pi i

from W 1ik k

i

R

estimated new state

1..2

,1

i L

kcalculae

from X Pxxk k

0..2'

i

k

i LCalc

observation

from

1X k k

xx kP

0

kX

1X k k

1Pk k

W weightsi

1

1

1i

k k

Calculate X k k

Pxxk k

from

Q

Pxy

Pyy

y

1xy

Calculate

K P Pyy K

1k kyyP

1k kxyP

GPS measures

1

1 1

:

ˆ( )

k

k k k k k k

EstimateX

X X K y y

y

1 1

:xx

T

K xx k k k yy k

Estimate P

P P K P K

1X k k

y

K1X k

1k kyyP

1Pxxk k

1Pxxk k

1Pxxk k

1xx

Pk

0 1.. 1..2a i L a a i L L a ak k k k k k k k

i i Lx x P x P

8

הפתרון הנבחר

צעד סימולציה )ע"פ שלבים(•איטרצי

ה

חילוץ המדידות

הרעשת המדידות

ייצור מסלול מדויק

-1.5 -1 -0.5 0 0.5 1 1.5

-1.5

-1

-0.5

0

0.5

1

1.5

simulated route

accurate route

1

1

1cos sin

1sin cos

11

110

k k

k k k k k

T

T

Tx x u

T

T

cos sin

sin cos

1

k kimuk k k ky u

Y_imu = Y_imu_accurate + b + v

מסלול אמיתי + מסלול מורעש

9

תוצאותסימולציה – מסלול פשוט•

10

תוצאותסימולציה – מסלול מתקדם•

11

תוצאותסימולציה – פרמטרי שגיאה לא •

נכונים

12

תוצאות1מדידות – מסלול •

13

תוצאות2מדידות – מסלול •

14

תוצאותמדידות – מסלול למתקדמים•

15

תוצאותשערוך פרמטרי תיקון•

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

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

תגובת יתר לשערוך ההיסט פרמטרים שגויים- מע' לא יציבה

-1 0 1 2-1

-0.5

0

0.5

1

real

UKF

-10 0 10 20 300.8

1

1.2

1.4

1.6final values: |P|=1

-5 0 5 10 15 20 25-10

-5

0

5

10final values: b=4.7342 -2.7418 0.0000

bx

bybfi

-0.1 0 0.1 0.2 0.3-0.05

0

0.05

0.1

0.15

0.2

real

UKF

-10 0 10 20 3010

15

20

25final values: |P|=20.1388

-5 0 5 10 15 20 25-1

-0.5

0

0.5

1final values: b=-0.4104 0.2856 0.0000

bx

bybfi

16

.O.Sפרמטרים תקינים עם שערוך ללא

-0.1 0 0.1 0.2 0.3-0.05

0

0.05

0.1

0.15

0.2

real

UKF

-10 0 10 20 30200

250

300

350

400

450final values: |P|=415.6643

-5 0 5 10 15 20 25-0.6

-0.4

-0.2

0

0.2

0.4final values: b=-0.4079 0.2988 0.0000

bx

bybfi

תוצאות

17

תוצאותGPSבעיות מקרי קצה – •

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

18

סיכום ומסקנות - הגדרנו את מטריצות הקווריאנס הבאות:UKFשני מימושים ל-•

ל- בשלב החיזוי לחישוב הסטטיסטיקות החדשות.R ל- ואת Q- הוספת המימוש הנבחר•- ניתן להרחיב את וקטור המצב ל:IIמימוש •

כוללות גם את הרעש(. SP עבור המצב החדש ועבור המדידה הבאה )כאשר ה-SP ואז למצוא את ה- לא התאים כיוון שקיבלנו מטריצת קווריאנס משוערכת שאינה מוגדרת חיובית. IIהמימוש ה-•IMUשיערוך תיקון שגיאות •

1|xxk kP

_ _ _9 9

xy Vxy xy Vxy V b xy b xy bQ diag q q q q q q q q q

xy

xy

R

yykP

0

xxk ka a k

k k

x x Px P

E W Q

0

xxk ka a k

k k

x x Px P

E V R

אפשר לצאת מהבית•

19

תודותהיינו רוצים להודות לאנשים הבאים :•

ד"ר גבי דוידוב על הליווי המקצועי לאורך הפרויקט : –מההסברים בנוגע לבניית המערכת ועד לעזרה בפתרון

בעיות.

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

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

פרופ' אריה פואייר על הייעוץ שניתן בנוגע לקלמן פילטר.–

פרופ' יעקוב אושמן על עזרתו.–