ניווט אינרציאלי באמצעות UKF

UKFניווט אינרציאלי באמצעות

ניווט אינרציאלי באמצעות

Unscented Kalman Filter

מגישיםניר בן זריהם גיא רוזנטל

מנחה ד"ר גבי דוידוב7.11.10תאריך:

אביב תש"ע

UKFניווט אינרציאלי באמצעות

( ניתן לנווט בעזרת מערכת GPSבהיעדר אמצעי ניווט חיצוני )

( המותקנת על כלי הרכבIMUניווט עצמונית )

הצגת הבעיה

מערכת זו כוללת:

מדי תאוצה13.

(GYRO מדי מהירות זוויתית )23.

.3Odometerמד מהירות גלגלים –

מתוך מדידות אלו וידיעת המיקום ההתחלתי ניתן לשערך את מיקום הרכב בכל זמן

UKFניווט אינרציאלי באמצעות

ניווט אינרציאלי - איך זה עובד?

ניתן לחשב ע"י NED): בהינתן תאוצות בצירי הניווט )הנחהאינטגרציה אחת את המהירויות באותם צירים, וע"י אינטגרציה

נוספת לשערך את המיקום במרחב

נתונות בצירי גוף, לכן יש IMU: התאוצות הנמדדות ע"י ה- בעיהצורך להעבירן לצירי ניווט

UKFניווט אינרציאלי באמצעות

טרנספורמציית הסיבוב

המצב הזוויתי מגדיר את סיבוב הגוף ביחס למערכת צירי הניווט ע"י טרנספורמציית הסיבוב:

1 0 0 cos( ) 0 sin( ) cos( ) sin( ) 0

0 cos( ) sin( ) 0 1 0 sin( ) cos( ) 0

0 sin( ) cos( ) sin( ) 0 cos( ) 0 0 1BODY NEDD

UKFניווט אינרציאלי באמצעות

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

Body frameTo Navigation frame NEDa Positionbodya

UKFניווט אינרציאלי באמצעות

IMU

AttitudeCalculation

rotation bodya

body

NEDaPosition

VelocityTrasformation

Matrix

Attitude

ניווט אינרציאלי – סכימת בלוקים

UKFניווט אינרציאלי באמצעות

משוואות ניווט

כל מערכות הניווט האינרציאלי צומחות מתוך סט המשוואות הבא:

NED accelerations

Earth-NED rotation

Downward velocity

NED-BODY rotation

UKFניווט אינרציאלי באמצעות

סביבונים. סביבונים אלו מושפעים 3המצב הזוויתי של הרכב נמדד ע"י מסיבוב כדה"א.

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

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

h

לכן בווקטור המצב נשמרות קואורדינאטות המיקום על כדור הארץ

. האורך הגיאוגרפי1

. הרוחב הגיאוגרפי2

. גובה מעל פני הים3

השפעות כדור הארץ

UKFניווט אינרציאלי באמצעות

תיקוני השפעת כדה"א

ערוץ הסביבונים

cos cos

sin sin

L LIB L

LIL

D dt

UKFניווט אינרציאלי באמצעות

תיקוני השפעת כדה"א

ערוץ מדי התאוצה

N N

E E

D D

V a

V a

V a

NR

2

(2 ) sin( ) cos

(2 )( sin cos )( ) cos

2(2 ) cos (1 0.0052884sin )(1 )

( ) cos

N ED E

N E

EN D

E

N EN E

N E e

V VV V

R h R h

VV V

R h

V V hV V g

R h R h R

* - radius of curvature in the prime vertical

UKFניווט אינרציאלי באמצעות

בעיית הרעש

: במצב של כניסות אידיאליות, ניתן לחשב במדויק את הנחה•מסלול הגוף ע"י שימוש במודל הפיזיקאלי

: בגלל שהמדידות טבולות ברעשים, המסלול שיחושב בעיה•בעזרת המודל הפיזיקאלי יתבדר במהירות בהשוואה למסלול

האמיתי

: כדי לקבל תוצאות נאמנות למציאות יש להכניס אמצעי פתרון•Kalman Filterבקרה לאלגוריתם, לכן משתמשים ב

UKFניווט אינרציאלי באמצעות

Kalman Filter•Kalman Filter )הוא משערך הנעזר במדידות חיצוניות )אובזרבלים

כדי להעריך את שגיאת החיזוי שלו ולעדכן את ווקטור המצב בהתאם

בגלל שהטרנספורמציה לחישוב המצב הבא אינה ליניארית, יש •להשתמש במסנן מתאים:

Extended Kalman Filter קידום ווקטור המצב נעשה בצורה - אנליטית, ע"י ליניאריזציה מסדר ראשון של המערכת

Unscented Kalman Filter )UKF( מבצע שערוך סטטיסטי של -המודל עד סדר שני )מתבסס על ההנחה שקל יותר לשערך

התפלגות הסתברותית מאשר לשערך מודל לא ליניארי(

UKFניווט אינרציאלי באמצעות

חיזוי המצב הבא

UKFניווט אינרציאלי באמצעות

חיזוי האובזרוול

UKFניווט אינרציאלי באמצעות

תיקון החיזוי ע"י מדידת האובזרוול

UKFניווט אינרציאלי באמצעות

Old state Create sigma points

Propagate each sigma point through the nonlinear model

Create prediction for next state

Fix prediction in accordance to extra

measurementNext state

UKF סכימת בלוקים -

UKFניווט אינרציאלי באמצעות

שלבי הפרויקט

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

simulator1

V

V

UKFניווט אינרציאלי באמצעות

שלבי הפרויקט

.1ב. בניית הסימולטור ההפוך: מבוסס על הטרנספורמציות ההפוכות מסימולטור

)מדי תאוצה וסביבונים(IMU מחשב מסלול התקדמות מתוך תפוקות ה-

simulator1

V

V Reverse

simulationR

UKFניווט אינרציאלי באמצעות

שלבי הפרויקט

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

simulator1

V

V Reverse

simulationR

( , )N E

( , )a aN E

UKFניווט אינרציאלי באמצעות

שלבי הפרויקט

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

simulator1

V

V Reverse

simulationR

( , )N E

( , )a aN E

UKF R

UKFניווט אינרציאלי באמצעות

תוצאותכחול- מסלול אמיתי1.

UKFאדום- מסלול משוערך עם 2.

ירוק- מסלול ללא סינון רעשים3.

UKFניווט אינרציאלי באמצעות

תוצאות

0 200 400 600 800 1000 1200-3000

-2500

-2000

-1500

-1000

-500

0

500

East

Nor

th

real

predicted

UKFניווט אינרציאלי באמצעות

תוצאות

חסינות לרעששחור –מסלול אמיתי

כחול- רעש מסדר גודל של הכניסה

אדום- רעש מסדר גודל של מהכניסה

ירוק- רעש מסדר גודל של מהכניסה

-200 0 200 400 600 800 1000 1200-3000

-2500

-2000

-1500

-1000

-500

0

500

East[m]

Nor

th[m

]

1e0 noise

1e-1 noise1e-2 noise

no noise

1

10

1

100

UKFניווט אינרציאלי באמצעות

מסקנות

הוכיח את יעילותוUKFאלגוריתם ה- •

האלגוריתם גמיש ומאפשר כיול עדין בהתאם לסוג הבעיה•

מודל כדה"א לא הראה שיפור משמעותי עבור המסלולים •הנתונים

מודל כד"הא הראה חסינות רבה לרעשים•

UKFניווט אינרציאלי באמצעות

הצעות למחקר נוסף

הוספת אובזרוול מד גובה ברומטרי•

עדכון עצירה מוחלטת•

בדיקת האלגוריתם עבור מסלול עם טווח השתנות בקנה מידה •ארצי