1 mobile robot localization and mapping using the kalman filter استاد درس: دکتر شيري...

11

Mobile RobotMobile RobotLocalization and MappingLocalization and Mapping

using the Kalman Filterusing the Kalman Filter

يياستاد درس: دکتر شيراستاد درس: دکتر شيرارائه کننده: مجيد نم نباتارائه کننده: مجيد نم نبات

خدا بنام

22

فهرستفهرست

ييمعرفمعرفموقعيت يابی رباتموقعيت يابی ربات

موقعيت يابی احتماالتیموقعيت يابی احتماالتی((LKFLKF))فيلترهای کالمن فيلترهای کالمن

Perturbation Kalman FilterPerturbation Kalman FilterExtended Kalman FilterExtended Kalman Filter

بهبودهابهبودهامراجعمراجع

33

ييمعرفمعرف

Rudulf Emil KalmanRudulf Emil Kalman ييلتر کالمن توسط آقالتر کالمن توسط آقاييفف ارائه شد. ارائه شد. 19601960در سال در سال

است. است.HMMHMMه ه ييشبشبن ن يينه است و کمترنه است و کمتريين زننده بهن زننده بهييک تخمک تخمييلتر لتر يين فن فيياا•

را دارد. را دارد.يين مربعن مربعييانگانگيي م مييخطاخطادار است.دار است.ييط مختلف پاط مختلف پاييدر شرادر شرا•ز مشخص ز مشخص يين را نن را نيينان هر تخمنان هر تخمييزان اطمزان اطمييمم•

انس(.انس(.ييکند) وارکند) وارييمم

44

لتر کالمنلتر کالمنييکاربردهای فکاربردهای ف

ر موشکهار موشکهايي مس مسييررييگگييپپ

اايير اشر اشيي دست، سر و سا دست، سر و ساييررييگگييپپ

ووييدديياستخراج حرکت لبها از واستخراج حرکت لبها از و

يي و ناوبر و ناوبرييدر مسائل اقتصاددر مسائل اقتصاد

55






66

ربات رباتييابابييت ت ييموقعموقع

ييک سرک سرييت ربات با استفاده از ت ربات با استفاده از ييص موقعص موقعييتشختشخاطالعاتاطالعات

ططيينقشه محنقشه مح

((ActsActs)) ربات ربات يياطالعات حرکتاطالعات حرکت

. انواع . انواع ((SensesSenses)) سنسورها سنسورها ييمشاهدات: خروجمشاهدات: خروجمشاهدات:مشاهدات:

خطا در مراحل مختلف حرکت ربات خطا در مراحل مختلف حرکت ربات يي: انباشتگ: انباشتگيي نسب نسبيي آيد. مثال ادومتر آيد. مثال ادومترييبوجود مبوجود م

باشد. باشد. يي م مييمطلق: در هر بار خطا مستقل از مشاهدات قبلمطلق: در هر بار خطا مستقل از مشاهدات قبلهاهاLandmarkLandmarkهمچون همچون

77

مميي با عال با عالييابابييت ت ييموقعموقع

م م يي عال عالييک سرک سرييت ت ييربات با دانستن موقعربات با دانستن موقع((LandmarksLandmarks))ن ن ييييت خود را تعت خود را تعييط موقعط موقعيي در مح در محکند.کند.ييمم

م:م:ييانواع عالانواع عالت بصورت فعالت بصورت فعالييفعال: فرستادن اطالعات موقعفعال: فرستادن اطالعات موقع

ر فعال: ربات با استفاده از اطالعات سنسورها ر فعال: ربات با استفاده از اطالعات سنسورها ييغغکند.کند.ييدا و مشخص مدا و مشخص مييآنها را پآنها را پ

88

مثالمثال

99

((MappingMapping)) ربات رباتيينقشه کشنقشه کش

ن ن ييييط را بگردد و ساختار آن را تعط را بگردد و ساختار آن را تعييد محد محييربات باربات باکند.کند.

ت ربات ت ربات يين موقعن موقعييييط با تعط با تعيين ساختار محن ساختار محييييتعتعشود.شود.ييمعموال انجام ممعموال انجام م

همزمان اصطالحا همزمان اصطالحا ييابابييت ت يي و موقع و موقعييبه نقشه کشبه نقشه کشSLAMSLAMشود.شود.يي گفته م گفته م

1010






1111

ييموقعيت يابموقعيت ياب)اجزا()اجزا(يياحتماالتاحتماالت

BeliefBelief ت ت ييک موقعک موقعيي: باور و گمان ربات به بودن در : باور و گمان ربات به بودن درکند. به تمام موقعيتهای محيط کند. به تمام موقعيتهای محيط ييخاص را مشخص مخاص را مشخص م

يک احتمال نسبت داده ميشود.يک احتمال نسبت داده ميشود.

Prior BeliefPrior Beliefن ربات از ن ربات از يين(: تخمن(: تخمييششيين پن پيي )تخم )تخم ييت خود قبل از در نظر گرفتن و بررست خود قبل از در نظر گرفتن و بررسييموقعموقع

باشد. باشد.ييت مت ميين موقعن موقعييمشاهدات مطلق در آخرمشاهدات مطلق در آخر

)|()( ...0 kkk dxPxBel

اطالعات اطالعات ربات تا ربات تا

کنونکنون

ده آل در ده آل در ييدر حالت ادر حالت اه ه يي و بق و بق11ت ربات ت ربات ييموقعموقع

.. باشد باشديي م م00موارد موارد

1212

مدل)ادامه( مدل)ادامه(يياجزااجزا

Posterior BeliefPosterior Beliefن ربات از ن ربات از يين(: تخمن(: تخميين پسن پسيي )تخم )تخم ييت خود بعد از در نظر گرفتن و بررست خود بعد از در نظر گرفتن و بررسييموقعموقع

باشد. باشد. ييت مت ميين موقعن موقعييمشاهدات مطلق در آخرمشاهدات مطلق در آخر

StateState)شامل شامل يي بردار بردار:: ) حالت ربات( ) حالت ربات nnر در ر در يي متغ متغاز ربات و از ربات و يي مورد ن مورد نييهر زمان است که ويژگيهاهر زمان است که ويژگيها

تواند تواند ييکند. بطور مثال مکند. بطور مثال مييسيستم را توصيف مسيستم را توصيف مت و جهت ربات باشد.ت و جهت ربات باشد.ييشامل موقعشامل موقع

1313

مدل)ادامه( مدل)ادامه(يياجزااجزا

Motion ModelMotion Modelربات(: با هر ربات(: با هر يي ) مدل حرکت ) مدل حرکت و مدل و مدل شودشودييت آن عوض مت آن عوض مييت ربات موقعت ربات موقعييفعالفعال

ان ميشود: ان ميشود: ييحرکت ربات بصورت زير بحرکت ربات بصورت زير ب

Sensor ModelSensor Model مدل سنسور ربات(: در هر )مدل سنسور ربات(: در هر( توانند توانند يي مشاهدات م مشاهدات مييک سرک سريي سنسورها سنسورها ييتتييموقعموقع

داشته باشند که بصورت زير بيان ميشود:داشته باشند که بصورت زير بيان ميشود:

),|( 11 kkk axxP

ت ت ييموقعموقعييفعلفعل

ت ت ييموقعموقعييقبلقبل

حرکت حرکت ((actionaction))

)|( kk xsP

1414

اجزا مدل)ادامه(اجزا مدل)ادامه(

Initial Belief stateInitial Belief stateه ه ييه(: گمان اوله(: گمان اولييت اولت اوليين موقعن موقعيي) تخم) تخم باشد. باشد.ييت خودش مت خودش ميي از موقع از موقعt=0t=0ربات در زمان ربات در زمان

ه:ه:يين اولن اوليي بسته به تخم بسته به تخمييابابييت ت يي موقع موقعييانواع روشهاانواع روشهاPosition TrackingPosition Trackingه کامال مشخص است.ه کامال مشخص است.ييت اولت اوليي: موقع: موقع

Global LocalizationGlobal Localizationه نامشخص است و ه نامشخص است و ييت اولت اوليي: موقع: موقعکسان است.کسان است.ييتها تها يياحتمال بودن در تمام موقعاحتمال بودن در تمام موقع

Kidnapped RobotKidnapped Robot : :نسبت به نسبت به ييشترشترييت احتمال بت احتمال بييک موقعک موقعيي گر دارد.گر دارد.يي د دييتهاتهاييموقموق

1515

ييابابييت ت ييالگوريتم موقعالگوريتم موقع

BayesianBayesianن ن ييک مساله تخمک مساله تخميي بصورت بصورت ييابابييت ت ييموقعموقعشود.شود.ييمدل ممدل م

در هر حالت ربات گمان حالت قبل خود را با استفاده در هر حالت ربات گمان حالت قبل خود را با استفاده کند.کند.يي م ميي خود بروز رسان خود بروز رسانيي و قطع و قطعيياز مشاهدات نسباز مشاهدات نسب

مدل حرکت را بصورت مدل مارکو در مدل حرکت را بصورت مدل مارکو در يي سادگ سادگييبرابرام: م: ييررييگگيينظر منظر م

و قانون بيز و قانون بيز Total ProbabilityTotal Probability ييبا استفاده از تئوربا استفاده از تئور آوريم: آوريم:ييبدست مبدست م

1616

)ادامه()ادامه(ييابابييت ت ييالگوريتم موقعالگوريتم موقع

ن بودن در برابر با احتمال ن بودن در برابر با احتمال ييششييطبق معادله باال گمان پطبق معادله باال گمان پآمدن از به ضربدر احتمال بودن در حالت با آمدن از به ضربدر احتمال بودن در حالت با

باشد. باشد.يي م مييتوجه به مشاهدات قبلتوجه به مشاهدات قبلن از فرض مارکو استفاده ن از فرض مارکو استفاده يين پسن پسيي بدست آوردن تخم بدست آوردن تخمييبرابرام:م:ييکنکنييمم

ن بصورت زير ن بصورت زير يين پسن پسييبا استفاده از قانون بيز، مقدار تخمبا استفاده از قانون بيز، مقدار تخم

آيد: آيد: ييبدست مبدست م

باشد. باشد.ييزه کننده برابر با مزه کننده برابر با مييک ثابت نرمالک ثابت نرماليي

1717

BeliefBeliefانواع انواع

وسته وجود دارد. در نوع گسسته الزم وسته وجود دارد. در نوع گسسته الزم ييدو نوع گسسته و پدو نوع گسسته و پ يي شود. روشها شود. روشهاييت گسسته سازت گسسته سازيي موقع موقعيياست که فضااست که فضا

وجود دارد: وجود دارد:يي گسسته ساز گسسته سازيي برا براييمختلفمختلف با محدود کردن مجموعه با محدود کردن مجموعه يي: گسسته ساز: گسسته سازيي مکان مکانييگرافهاگرافها

شود.شود.يي ربات انجام م ربات انجام مييتهاتهاييفعالفعال

ييک دقت خاص شبکه بندک دقت خاص شبکه بندييت را با ت را با يي موقع موقعيي: فضا: فضاييشبکه بندشبکه بندکنند.کنند.ييمم

Particle FilterParticle Filterيي: گمان با استفاده از مجموع وزن: گمان با استفاده از مجموع وزن mm نمونه توزيع نمونه توزيع شود.شود.ييان مان مييشده بشده ب

Monte Carlo Monte Carlo يي موقعيت ياب موقعيت يابييها براها براParticle FilterParticle Filterاستفاده از استفاده از LocalizationLocalization.ناميده ميشود. ناميده ميشود

1818






1919

کالمن کالمنييلترهالترهاييفف

کنند.کنند.يي فرض م فرض ميي را بصورت گوس را بصورت گوسBeliefBeliefتابع تابع

گوس يتوز يبعد 1 يع گوس يتوز يبعد n يع

2020

LKFLKF مدل مدل ييفرضهافرضها

فرض ميکنند ديناميک فرض ميکنند ديناميک يي کالمن خط کالمن خطييفيلترهافيلترها بودن، بودن، يي باشد. منظور از خط باشد. منظور از خطييسيستم خطسيستم خط

يي بودن معادالت مدل حرکت و سنسور م بودن معادالت مدل حرکت و سنسور مييخطخطباشد.باشد.

مدل حرکت مدل حرکت

مدل سنسور مدل سنسور

نامند؟ نامند؟ييلتر ملتر مييچرا فچرا ف

11 kkk wAxxAي ماتريس است که حالت

را به يفعل مرتبط يقبل

ميکند.kkk vHxz

مشاهدات Hماتريس ربات را يسنسورها مختلف يدر حالتها

بدون در نظر گرفتن نويز نشان ميدهد.

بها ي ندل ي لکهمشاهداتنويزداررا در ي افتم ي کندوسپسسع يم ي کند

موقع ي تصح ي حراتخم ي نبزندبهآن يف لترم ي گو ي ند.

2121

)ادامه()ادامه(LKFLKF مدل مدل ييفرضهافرضها

منابع نويز مدل حرکت و مدل سنسور مستقل از يکديگر، منابع نويز مدل حرکت و مدل سنسور مستقل از يکديگر، با ميانگين صفر در نظر گرفته ميشوند: با ميانگين صفر در نظر گرفته ميشوند:ييسفيد و گوسسفيد و گوس

با ميانگين و ماتريس با ميانگين و ماتريس يي بصورت گوس بصورت گوسBeliefBeliefهر هر مشخص ميشود. مشخص ميشود.ييکوواريانس توزيع گوسکوواريانس توزيع گوس

کالمن کالمن يي فيلترها فيلترهايي بدليل در نظر گرفتن توزيع گوس بدليل در نظر گرفتن توزيع گوسUnimodelUnimodelباشند و الزم است که در ابتدا موقعيت باشند و الزم است که در ابتدا موقعيت يي م م

اوليه ربات مشخص باشد.اوليه ربات مشخص باشد.

),0(~

),0(~

kk

kk

RNv

QNw

),ˆ()(

),ˆ()(

kkk

kkk

PxNxBel

PxNxBel ميزان اطمينان تخمين

موقعيت تخمين زده شده

2222

مثالمثال

ss است که در يک جهت با سرعت ثابت است که در يک جهت با سرعت ثابت ييفرض کنيد رباتفرض کنيد رباتحرکت کند. مدل سيستم ربات برابر است با: حرکت کند. مدل سيستم ربات برابر است با:

با ميانگين صفر و با ميانگين صفر و يينويز مدل سيستم توزيع گوسنويز مدل سيستم توزيع گوسواريانس در نظر گرفته ميشود.واريانس در نظر گرفته ميشود.

ممکن ممکن يي مدل کردن خروجيها مدل کردن خروجيهاييمدل سنسور که برامدل سنسور که براسنسورها در يک حالت خاص بکار ميرود را نيز بصورت سنسورها در يک حالت خاص بکار ميرود را نيز بصورت

زير در نظر ميگيريم:زير در نظر ميگيريم:

تخمين اوليه موقعيت را نيز با واريانس در نظر تخمين اوليه موقعيت را نيز با واريانس در نظر ميگيريم.ميگيريم.

kkk wsxx 1

w

kkk vxz

0x02

2323

مثال)ادامه(مثال)ادامه(

يي دانيم، لذا فقط مدل تقريب دانيم، لذا فقط مدل تقريبييما مقدار نويز را نمما مقدار نويز را نمحرکت و سنسور را با در نظر گرفتن نويز صفر حرکت و سنسور را با در نظر گرفتن نويز صفر

ميتوانيم داشته باشيم:ميتوانيم داشته باشيم:

حرکت کند، حرکت کند، يياکنون اگر ربات از محل اوليه يک گام زماناکنون اگر ربات از محل اوليه يک گام زمانموقعيتموقعيت

ربات تقريبا برابر با مقدار زير ميشود:ربات تقريبا برابر با مقدار زير ميشود:

ميزان دقت تخمين برابر است با: ميزان دقت تخمين برابر است با:

01 sxx kk

0 kk xz

001 sxx

220

21 w

2424

مثال)ادامه(مثال)ادامه(

بعد از پيشگويي موقعيت جديد، اکنون الزم است بعد از پيشگويي موقعيت جديد، اکنون الزم است سنسورها خوانده شود و با توجه به سنسورها خوانده شود و با توجه به ييکه خروجکه خروج

مقادير مشاهدات موقعيت پيشگويي شده اصالح مقادير مشاهدات موقعيت پيشگويي شده اصالح شود.شود.

با با يي الزم است که مقادير مشاهدات را بگونه ا الزم است که مقادير مشاهدات را بگونه ا يي از موقعيت ترکيب کنيم. برا از موقعيت ترکيب کنيم. براييمدل تخمينمدل تخمين

ميتوان استفاده کرد: ميتوان استفاده کرد:يياينکار از معيار وزناينکار از معيار وزن

11 xz

2525

مثال )ادامهمثال )ادامه))

کرد: کرد:ييفرمول باال را ميتوان بصورت زير بازنويسفرمول باال را ميتوان بصورت زير بازنويس

!! جالب ميباشد جالب ميباشدKK رفتار رفتار ييبررسبررسKFKF .در دو مرحله کار ميکند:پيشگويي، اصالح. در دو مرحله کار ميکند:پيشگويي، اصالح

ابتدا با مدل سيستم موقعيت را پيشگويي ميکند ابتدا با مدل سيستم موقعيت را پيشگويي ميکند و سپس با مدل سنسور موقعيت پيشگويي شده و سپس با مدل سنسور موقعيت پيشگويي شده

را اصالح ميکند.را اصالح ميکند.

2626

و ستاره و ستارهييمثال کشتمثال کشت

ييييشگوشگوييمرحله پمرحله پ

مرحله اصالحمرحله اصالح

2727

سيستم و سيستم و ييمدل گوسمدل گوسسنسورسنسور

الزم است که و الزم است که و BeliefBelief محاسبه محاسبه ييبرابرا اينکار از مدل سيستم و مدل اينکار از مدل سيستم و مدل ييمحاسبه شوند. برامحاسبه شوند. برا

سنسور استفاده ميکنيم. سنسور استفاده ميکنيم.

pع گوسpتوزي يpک بصpورت ميتpوان را توزيpع گوسpضريب يpک بصpورت ميتpوان را بpا ييضريب بpا ميpانگين و واريpانس صpفر در نظpر گpرفت. لpذا ميpانگين و واريpانس صpفر در نظpر گpرفت. لpذا

آوريم: آوريم:ييبدست مبدست م

آوريم: آوريم:ييبطور مشابه بدست مبطور مشابه بدست م

)|( 1kk xxP)|( kk xzP

1kAx 1kAx

2828

((ييييشگوشگويين)گام پن)گام پييششيين پن پييتخمتخم

Prior Estimation Error

2929

ن) گام اصالح(ن) گام اصالح(يين پسن پسييتخمتخم

مراحل کار:مراحل کار:ک فضا ک فضا يي متفاوت به متفاوت به يي از فضاها از فضاهاييع گوسع گوسييبردن دو توزبردن دو توز

ييع گوسع گوسييب دو توزب دو توزييترکترکييساده سازساده ساز

کالمن Kalman(بهرهGain(

3030

معادالت معادالتييبررسبررس

ن ن ييانس مرحله قبل به اانس مرحله قبل به ايي وار وارييييشگوشگوييدر مرحله پدر مرحله پن ن يي و همچن و همچن((PropagatePropagate))ابد ابد يي ييمرحله انتقال ممرحله انتقال م

شود.شود.ييز در نظر گرفته مز در نظر گرفته مييانس نويز سيستم نانس نويز سيستم نييواروار

3131

معادالت)ادامه( معادالت)ادامه(ييبررسبررس

لpتر کpالمن انتظpار دارد کpه مشpاهدات مطpابق بpا لpتر کpالمن انتظpار دارد کpه مشpاهدات مطpابق بpا ييففتخمييمpوقعمpوقع تخمت صpفر ييننيpيpت نpويز گpرفتن درنظpر بpا صpفر نpويز گpرفتن درنظpر بpا باشد:باشد:

م را م را بصpورت يي بصpورت تpوان متغpيpيpتpوان متغpک تصpادفييک تصpادفر بpا يير بpا ر در نظر گرفت:ر در نظر گرفت:يي ز زييع گوسع گوسييتوزتوز

kz

3232


ايpيpاا آل اده آل مييده تفpاوت کpه اسpت من تفpاوت کpه اسpت مشpاهدات يpيpن مشpاهدات ان ان اگpر اگpر . . مpورد نظpر صpفر باشpد مpورد نظpر صpفر باشpدييسنسpورها و خpروجسنسpورها و خpروجباشpد صpفر باشpد مانpده صpفر نييعpنعpنييمانpده ن pيpانييازازيpرس بpروز بpه رسpان بpروز بpه يي

پييتخمتخم پن نمييشpشpيين نمن تخميين و باشpد تخم و باشpد پيي پن بpا ييشpشpيين بpا ن ن نصpورت نصpورت ييراراييمشpاهدات کpامال مطpابقت دارد، در غمشpاهدات کpامال مطpابقت دارد، در غ

شود. شود.يين بروز رسانن بروز رسانييالزم است که تخمالزم است که تخم

ع ع يي با توز با توزيير تصادفر تصادفييک متغک متغييز ز ييمانده را نمانده را نييباقباق توان در نظر گرفت: توان در نظر گرفت:يي بصورت زير م بصورت زير مييگوسگوس

)Residual(مانده يباق يخطا

3333


ميpيpواروار باقيمانpده باقيمانpده مانس مpييانس عنpوان بpه مpتوانpد عنpوان بpه و ييتوانpد و زان خطpا زان خطpا رد.رد.يي سنسورpها مورpد استفpاده قراpر بگ سنسورpها مورpد استفpاده قراpر بگييز خروpجز خروpجيpيpنونو

تpوان آن را تpوان آن را يين و مشpاهدات من و مشpاهدات مييشpشpيين پن پييب تخمب تخميي تpرک تpرکييبرابراانس انس يpيpتpوان نسpبت وارpتpوان نسpبت وارpيين وزن را من وزن را ميpيp کpرد. ا کpرد. ايpيpجمpع وزنجمpع وزن

تعريياا تعرن pدو اpيpيpن pدو دpر کpرد. اpف دpر کpرد. وارييف حpالت وارن حpالت هpر يpيpن هpر انس ک ک يpيpانس ب سpهpم pدpباش pترpبکم سpهpم pدpباش pترpکمpيpتريpترشpخمييشpت دpر تpخم دpر نpهpايي نpهpان خواهpد يييين خواهpد

داشت.داشت.

نقش ؟نقش ؟TH

بردنوار ي انستخم ي ناول ي هازفضا ي حالتبهفضا مشاهدات ي

3434


بهره کالمن: بهره کالمن:يي حالت حد حالت حدييبررسبررس

ستم صفرستم صفرييانس سانس سييمانده موثر و وارمانده موثر و واريي : فقط باق : فقط باق00نويز مشاهدات = نويز مشاهدات = : ؟: ؟00ه = ه = يين اولن اولييانس تخمانس تخمييواروار

شود که شود که ييشود وثابت مشود وثابت مييده مده مييلتر کالمن ناملتر کالمن ناميي قلب ف قلب ف((KGKG)) بهره کالمن بهره کالمن KFKFل استفاده از ل استفاده از يي بدل بدلKGKG ستم با ستم با ييک سک سيي يينه برانه برايين زننده بهن زننده بهييک تخمک تخميي

ييانس ممکن را مانس ممکن را ميين وارن واريين با کمترن با کمتريي باشند و تخم باشند و تخميي م مييک خطک خطيينامنامييددزند.زند.

3535

بهره کالمنبهره کالمن

3636

لتر کالمنلتر کالمنيياگرام فاگرام فييبلوک دبلوک د

3737

الگوريتم تکراریالگوريتم تکراری

3838

مثال قبل مثال قبلييبررسبررسک جهت با سرعت ثابت ک جهت با سرعت ثابت ييم که در م که در ييک ربات داشتک ربات داشتييssکند.کند.يي حرکت م حرکت م

3939

State Estimation Error vs 3s Region of Confidence

4040

Sensor Residual vs 3s Region of Confidence

سpتم سpتم يي از انحpراف س از انحpراف سييرريي جلوگp جلوگpييشpود کpه بpراشpود کpه بpرايي ثpابت م ثpابت مييبصpورت تجpرببصpورت تجpرب باشد. باشد. ييناننانيpيpک محدوpدpه اpطمک محدوpدpه اpطميpيpد در pد در pيpيpالزpم است کpه مpاندpه همpوpارpه بpاالزpم است کpه مpاندpه همpوpارpه بpا

4141

22مثال مثال ه ه ييت اولت اوليينبار موقعنبار موقعييه مثال قبل است، فقط اه مثال قبل است، فقط اييشبشب

م:م:يي ده دهييستم مستم مييحرکت ربات را اشتباه به سحرکت ربات را اشتباه به س

4242

مشکالتمشکالت

يي دارا دارايي سنسورها همچون انکدرها و ادومتر سنسورها همچون انکدرها و ادومترييبعضبعض خطا هستند و الزم است که جز مدل خطا هستند و الزم است که جز مدل ييانباشتگانباشتگ

سيستم محسوب شوند.سيستم محسوب شوند.کpه جمالت اسpت کpه جمالتراه حpل: الزم اسpت مpدل سييراه حpل: الزم بpه مpدل س بpه اضpافه يي اضpافه سpتم سpتم

pب بدpسpت pارهpدوبp pادالتpعpم بpشpوpد pو بدpسpت pارهpدوبp pادالتpعpم ت pشpبيpيpااييشpوpد pو pدالpاpمع .pدpبنpشp ت pدالpاpمع .pدpنpيpه ه يم قبpل pادالتpعpمم قبpل pادالتpعpتييم pجمال و pفقpط ت pباشpند pجمال و pفقpط اضpاpفه يي pباشpند آنهpا بpه اضpاpفه آنهpا بpه

شود.شود.ييمم

خطmobile robotmobile robotشpتر شpتر ييبب بصpورت را خطهpا بصpورت را نمييهpا نم تpوان يي تpوان مدل کرد.مدل کرد.

((PKFPKF ) )ستمستمييک سک سيينامناميي مدل د مدل ديي ساز سازييخطخط. . ((EKFEKF))يير خطر خطيي غ غييستمهاستمهايي به س به سLKFLKFگسترش مدل گسترش مدل

4343






4444

Perturbation Kalman FilterPerturbation Kalman Filter

ميتpوانن ميتpوانن ييمدل سيسpتم وسنسpور در حpالت غpير خطمدل سيسpتم وسنسpور در حpالت غpير خطبصورت زير در نظر گرفته شوند:بصورت زير در نظر گرفته شوند:

خطييبرابرا خط سpازيي سpاز سيي مpدل س مpدل سpير يي خpط يpک سpير سpتم خpط يpک سpتم تعريpف ميشpود و سpپس تعريpف ميشpود و سpپس ((nominal trajectorynominal trajectory))نرمpال نرمpال

pر سpيpط سpين خpول اpحpر سpيpط سpين خpول اpتفاده از ييحpا اسpات بpربp رکتpتفاده از ر حpا اسpات بpربp رکتpر ح ميشوpد. ميشوpد. يpيpتقpريب درجه اوpل بسط تpيلور خطتقpريب درجه اوpل بسط تpيلور خط

4545

PKFPKF)ادامه()ادامه(

سpاز سpازيي خط خطييبرابرا غpير خطيي معادلpه مpدل سيسpتم، غpير خط معادلpه مpدل سيسpتم، يي p لتpاpه حpوط بpمرب p لتpاpه حpوط بpخط مرب pکيبpرpت pکpه يpخطرا ب pکيبpرpت pکpه يpط ييرا بpط از خpاز خ

pک جز آشفتگpو ي p لpسير نرماpک جز آشفتگpو ي p لpييسير نرما p p p p p.ودpتجزيه ميشp.ودpتجزيه ميش

باشد: باشد:يير مر ميير نرمال بصورت زر نرمال بصورت زييروش محاسبه خط سروش محاسبه خط س

م:م:ييکنکنيي بصورت زير عمل م بصورت زير عمل ميي محاسبه جز آشفتگ محاسبه جز آشفتگييبرابرا

kxnomkx 1

1 kx

4646

PKFPKF)ادامه()ادامه(

h.o.th.o.t..؟ ؟

جزئ n*n يماتريس Aس يماتر مشتقات .يم f ياز باشد

4747

PKFPKFالگوريتم الگوريتم

مدل سنسور نيز بطور مشابه عمل مدل سنسور نيز بطور مشابه عمل يي ساز سازيي خط خطييبرابراميکنيم.ميکنيم.

شوند و شوند و يي م مييمعادالت سيستم و سنسور بصورت زير خطمعادالت سيستم و سنسور بصورت زير خطم.م.ييکنکنيي استفاده م استفاده مLKFLKFسپس از معادالت سپس از معادالت

4848

PKFPKF روش روش ييبررسبررس

يي قبل از راه انداز قبل از راه اندازA,FA,Fامکان محاسبه ماتريس امکان محاسبه ماتريس ش ش ييتواند منجر به افزاتواند منجر به افزاييربات وجود دارد که مربات وجود دارد که م

سرعت محاسبات در حين حرکت ربات شود.سرعت محاسبات در حين حرکت ربات شود.

pخط سpداييخط سpابت ابتpدار نرمpال در نظpر گرفتpه شpده در يير نرمpال در نظpر گرفتpه شpده در ادp ادp يpيp ز زيي اpنباشpتگp اpنباشpتگpييقp باشpد pوگرنpه خطpاقp باشpد pوگرنpه خطpايpيpدp دقدp دقيpيpکpارp باکpارp با

ت pدرجpه pبpاال سpرييشpودp و دشpودp و دييمم pر جمالpرگpاال سpبp هpدرجp ت pر جمالpل يي ت تييگpور قابpل لpور قابpلpچشمپوشpند.يي نم نمييچشمپوشpند. باشpرا باشpرا بpراحيي بpراح طpدق دقيي ط pيpط يpط ق خpق خ

pسpداييسpال در ابتpدار نرمpال در ابتpات يير نرمpه اطالعpت کpار الزم اسpات کpه اطالعpت کpار الزم اسpک م.م.يير حرکت ربات داشته باشر حرکت ربات داشته باشييق از مسق از مسييدقدق

4949






5050

Extended Kalman FilterExtended Kalman Filter

در اين روش خط سير نرمال بصورت ديناميک در هر حالت در اين روش خط سير نرمال بصورت ديناميک در هر حالت تغيير تغيير يي قبل قبلييبا استفاده از اطالعات حالتها و سنسورهابا استفاده از اطالعات حالتها و سنسورها

ميکند.ميکند.

حول يک خط حول يک خط يي ساز سازيي خط خطييدر اين روش در هر حالت بجادر اين روش در هر حالت بجا يي ساز سازيي خط خط سير نرمال از قبل محاسبه شده، حولسير نرمال از قبل محاسبه شده، حول

انجام ميشود.انجام ميشود.

ر نرمال مشاهدات نيز بصورت زير در نظر گرفته ر نرمال مشاهدات نيز بصورت زير در نظر گرفته ييسسشود:شود:ييمم

1kx

5151

EKFEKF)ادامه()ادامه(

با در نظر گرفتن خط سير نرمال بصورت باال جز با در نظر گرفتن خط سير نرمال بصورت باال جز در مراحل پيشگويي و اصالح همواره در مراحل پيشگويي و اصالح همواره ييآشفتگآشفتگ

بدست بدست LKFLKF زير مشابه زير مشابه ييصفر ميشود و فرمولهاصفر ميشود و فرمولها آيند. آيند. ييمم

گام اصالح:گام اصالح:

::ييييشگوشگوييگام پگام پ

5252

EKFEKF)ادامه()ادامه(

F,HF,Hنبار در هر نبار در هر يي ژاکوبين هستند که ا ژاکوبين هستند که ايي ماتريسها ماتريسهاحالت الزم است که محاسبه شوند:حالت الزم است که محاسبه شوند:

5353

برای برای EKFEKFاستفاده از استفاده از موقعيت يابیموقعيت يابی

5454

Mobile RobotMobile Robotمدل يک مدل يک

R

X

Y

xy

G

vR

از ديد ربات معادالت حرکت از ديد ربات معادالت حرکت بصورت زير می باشند:بصورت زير می باشند:

tt

t

tt

y

Vx

0

از نظر مختصات کلی حرکت از نظر مختصات کلی حرکت بصورت زير ميتواند مدل شود:بصورت زير ميتواند مدل شود:

tt

ttt

ttt

Vy

Vx

sin

cos

مدل سيستم با در نظر گرفتن نويز مدل سيستم با در نظر گرفتن نويز بصورت زير ميتواند مدل شود:بصورت زير ميتواند مدل شود:

tw

twVyy

twVxx

t

t

t

ttt

tVttt

tVttt

)(ˆˆ

ˆsin)(ˆˆ

ˆcos)(ˆˆ

1

1

1

5555

Mobile Mobileمدل يک مدل يک RobotRobot)ادامه()ادامه(

اکنون الزم است که مدل سيستم را با استفاده اکنون الزم است که مدل سيستم را با استفاده خطی سازی کنيم: خطی سازی کنيم:EKFEKFاز از

111

111

111

~ˆ

~ˆ

~ˆ

ttt

ttt

ttt

yyy

xxx

برای خطی سازی سيستم الزم است که برای برای خطی سازی سيستم الزم است که برای زوايای کوچک فرضهای زير در نظر گرفته شود.زوايای کوچک فرضهای زير در نظر گرفته شود.

~~

sin

1~

cos

5656

Mobile Mobileمدل يک مدل يک RobotRobot)ادامه()ادامه(

مدل سيستم در انتها بصورت زير در می آيد:مدل سيستم در انتها بصورت زير در می آيد:

ttttt

V

R

R

t

t

t

m

m

t

t

t

WGXFX

w

w

t

t

t

y

x

tV

tV

y

x

t

t

~~

0

0sin

0cos

~~

~

100

ˆcos10

ˆsin01

~~

~

1

1

1

1

با توجه به مدل باال، واريانس پيشين مقدار زير با توجه به مدل باال، واريانس پيشين مقدار زير Tميشود:ميشود:

tttTtttttt GQGFPFP //1

5757

Mobile Mobileمدل سنسور برای يک مدل سنسور برای يک RobotRobot

از ديد رباتاز ديد ربات

از نظر مختصات عمومیاز نظر مختصات عمومی

اين مدل نيز الزم است که اين مدل نيز الزم است که خطی سازی شود.خطی سازی شود.

R

X

Y

xy

G

L

z

n

n

n

y

x

z y

x

L

L

L

t

t

t

t

1

1

1

1

n

n

n

yy

xx

z y

x

tL

tL

tL

tt

tt

t

t

t

t

1

1

1

11

11

1

1

1

1

100

0cossin

0sincos

5858

Mobile Mobileمدل سنسور برای يک مدل سنسور برای يک RobotRobot)ادامه( )ادامه(

بعد از محاسبه مدل زير بدست می آيد:بعد از محاسبه مدل زير بدست می آيد:

n

n

n

y

x

yyxx

yyxx

y

x

y

x

t

t

t

tLttLttt

tLttLtt

L

L

L t

t

t

t

1

1

1

11111

1111

~~

~

100

)ˆ(ˆsin)ˆ(ˆcosˆcosˆsin

)ˆ(ˆcos)ˆ(ˆsinˆsinˆcos

~~

~1

1

1

1

5959

EKF for SLAMEKF for SLAM

بردار حالت اينبار عالوه بر شامل شدن مختصات ربات بردار حالت اينبار عالوه بر شامل شدن مختصات ربات شامل مختصات عاليم نيز می باشد.شامل مختصات عاليم نيز می باشد.

بردار کوواريانس نيز بصورت زير در نظر گرفته ميشود:بردار کوواريانس نيز بصورت زير در نظر گرفته ميشود:

NNNN

N

N

LLLLRL

LLLLRL

RLRLRR

PPP

PPP

PPP

1

1111

1

مدل سنسور بصورت زير در می آيد:مدل سنسور بصورت زير در می آيد: 0000

~~~~~1

i

ni

LR

TL

TL

TL

TR

HHH

XXXXX

6060

EKF for SLAMEKF for SLAM

6161






6262

بهبودهابهبودها

Iterated EKFIterated EKFpبه تخمين پسpدر اين روش محاس :pبه تخمين پسpن ن يي: در اين روش محاستخم آورpدن ميpشpود. pبدسpت انpجpام بpار تخمچنpدين آورpدن ميpشpود. pبدسpت انpجpام بpار ن ن ييچنpدين

pپسpنييپسpنن مبتpتخميين مبتp رpتخم بp رpب pيpيpن پسpيييين پس pوجبpل مpرار قبpن تک pوجبpل مpرار قبpن تکشود.شود.يين من ميين پسن پسييش دقت تخمش دقت تخمييافزاافزا

GMMGMMم: م بجpايي: بجpاتpوان اييتpوان ا بصpورت يي را گمpان بصpورت نکpه را گمpان ک ک يpيpنکpه گوسpيpيpتوزتوز گوسpع بگpييع نظpر در pبگ نظpر در بصpورت ييرريي بصpورت م در GMMGMMم در

م.م.ييرريينظر بگنظر بگ

داده داده يي بررسp بررسpييلpتر بpرالpتر بpرايين کpالمن فن کpالمن فيياسpتفاده از چنpداسpتفاده از چنpدها با فرضيه های مختلفها با فرضيه های مختلف

6363

استفاده از چند فيلتر کالمناستفاده از چند فيلتر کالمن

6464






6565

مراجعمراجع

[1] R. Negenborn, Robot Localization and Kalman Filters on finding your position in a noisy world, Thesis for Master of Science,,2003.

[2] P.E. Rybski, Localization and Mapping using the Kalman Filter, CMRoboBits, 2002

[3] http://www.cs.unc.edu/~weltch/kalmanlinks.html

[4] http://www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html

http://www.cs.unc.edu/~weltch/kalmanlinks.html

http://www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html

6767

با با يي: يکپارچه ساز: يکپارچه ساز11وست وست ييپپمدل سنسورمدل سنسور

قانون بيز را اعمال ميکنيم:قانون بيز را اعمال ميکنيم:

با فرض مارکو داريم:با فرض مارکو داريم:

6868

با با يي: يکپارچه ساز: يکپارچه ساز11وست وست ييپپمدل سنسور)ادامه(مدل سنسور)ادامه(

مخرج صرفا يک نرماليزه کننده است که با فرمول زير محاسبه ميشود:مخرج صرفا يک نرماليزه کننده است که با فرمول زير محاسبه ميشود:

6969

KFKFن ن يين پسن پسيي: تخم: تخم22وست وست ييپپ

7070

KFKFن ن يين پسن پسيي: تخم: تخم22وست وست ييپپ)ادامه()ادامه(

1 mobile robot localization and mapping using the kalman filter استاد درس: دکتر شيري...

Documents