1 Mobile Robot Mobile Robot Localization and Localization and Mapping Mapping using the Kalman using the Kalman Filter Filter ر ي ش ر کي اد درس: د ت س ا ر ي ش ر کي اد درس: د ت س ا ي ي ات ت ن م ن د ي ج م ده: ت ن ک ه$ ئ ارا ات ت ن م ن د ي ج م ده: ت ن ک ه$ ئ ارا دا ام خ ت ن
71
Embed
1 Mobile Robot Localization and Mapping using the Kalman Filter استاد درس: دکتر شيري ارائه کننده: مجيد نم نبات بنام خدا.
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
11
Mobile RobotMobile RobotLocalization and MappingLocalization and Mapping
using the Kalman Filterusing the Kalman Filter
يياستاد درس: دکتر شيراستاد درس: دکتر شيرارائه کننده: مجيد نم نباتارائه کننده: مجيد نم نبات
Rudulf Emil KalmanRudulf Emil Kalman ييلتر کالمن توسط آقالتر کالمن توسط آقاييفف ارائه شد. ارائه شد. 19601960در سال در سال
است. است.HMMHMMه ه ييشبشبن ن يينه است و کمترنه است و کمتريين زننده بهن زننده بهييک تخمک تخمييلتر لتر يين فن فيياا•
را دارد. را دارد.يين مربعن مربعييانگانگيي م مييخطاخطادار است.دار است.ييط مختلف پاط مختلف پاييدر شرادر شرا•ز مشخص ز مشخص يين را نن را نيينان هر تخمنان هر تخمييزان اطمزان اطمييمم•
انس(.انس(.ييکند) وارکند) وارييمم
44
لتر کالمنلتر کالمنييکاربردهای فکاربردهای ف
ر موشکهار موشکهايي مس مسييررييگگييپپ
اايير اشر اشيي دست، سر و سا دست، سر و ساييررييگگييپپ
ووييدديياستخراج حرکت لبها از واستخراج حرکت لبها از و
يي و ناوبر و ناوبرييدر مسائل اقتصاددر مسائل اقتصاد
ييک سرک سرييت ربات با استفاده از ت ربات با استفاده از ييص موقعص موقعييتشختشخاطالعاتاطالعات
ططيينقشه محنقشه مح
((ActsActs)) ربات ربات يياطالعات حرکتاطالعات حرکت
. انواع . انواع ((SensesSenses)) سنسورها سنسورها ييمشاهدات: خروجمشاهدات: خروجمشاهدات:مشاهدات:
خطا در مراحل مختلف حرکت ربات خطا در مراحل مختلف حرکت ربات يي: انباشتگ: انباشتگيي نسب نسبيي آيد. مثال ادومتر آيد. مثال ادومترييبوجود مبوجود م
باشد. باشد. يي م مييمطلق: در هر بار خطا مستقل از مشاهدات قبلمطلق: در هر بار خطا مستقل از مشاهدات قبلهاهاLandmarkLandmarkهمچون همچون
77
مميي با عال با عالييابابييت ت ييموقعموقع
م م يي عال عالييک سرک سرييت ت ييربات با دانستن موقعربات با دانستن موقع((LandmarksLandmarks))ن ن ييييت خود را تعت خود را تعييط موقعط موقعيي در مح در محکند.کند.ييمم
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 : :نسبت به نسبت به ييشترشترييت احتمال بت احتمال بييک موقعک موقعيي گر دارد.گر دارد.يي د دييتهاتهاييموقموق
در هر حالت ربات گمان حالت قبل خود را با استفاده در هر حالت ربات گمان حالت قبل خود را با استفاده کند.کند.يي م ميي خود بروز رسان خود بروز رسانيي و قطع و قطعيياز مشاهدات نسباز مشاهدات نسب
مدل حرکت را بصورت مدل مارکو در مدل حرکت را بصورت مدل مارکو در يي سادگ سادگييبرابرام: م: ييررييگگيينظر منظر م
و قانون بيز و قانون بيز Total ProbabilityTotal Probability ييبا استفاده از تئوربا استفاده از تئور آوريم: آوريم:ييبدست مبدست م
1616
)ادامه()ادامه(ييابابييت ت ييالگوريتم موقعالگوريتم موقع
ن بودن در برابر با احتمال ن بودن در برابر با احتمال ييششييطبق معادله باال گمان پطبق معادله باال گمان پآمدن از به ضربدر احتمال بودن در حالت با آمدن از به ضربدر احتمال بودن در حالت با
باشد. باشد.يي م مييتوجه به مشاهدات قبلتوجه به مشاهدات قبلن از فرض مارکو استفاده ن از فرض مارکو استفاده يين پسن پسيي بدست آوردن تخم بدست آوردن تخمييبرابرام:م:ييکنکنييمم
ن بصورت زير ن بصورت زير يين پسن پسييبا استفاده از قانون بيز، مقدار تخمبا استفاده از قانون بيز، مقدار تخم
آيد: آيد: ييبدست مبدست م
باشد. باشد.ييزه کننده برابر با مزه کننده برابر با مييک ثابت نرمالک ثابت نرماليي
1717
BeliefBeliefانواع انواع
وسته وجود دارد. در نوع گسسته الزم وسته وجود دارد. در نوع گسسته الزم ييدو نوع گسسته و پدو نوع گسسته و پ يي شود. روشها شود. روشهاييت گسسته سازت گسسته سازيي موقع موقعيياست که فضااست که فضا
وجود دارد: وجود دارد:يي گسسته ساز گسسته سازيي برا براييمختلفمختلف با محدود کردن مجموعه با محدود کردن مجموعه يي: گسسته ساز: گسسته سازيي مکان مکانييگرافهاگرافها
شود.شود.يي ربات انجام م ربات انجام مييتهاتهاييفعالفعال
ييک دقت خاص شبکه بندک دقت خاص شبکه بندييت را با ت را با يي موقع موقعيي: فضا: فضاييشبکه بندشبکه بندکنند.کنند.ييمم
Particle FilterParticle Filterيي: گمان با استفاده از مجموع وزن: گمان با استفاده از مجموع وزن mm نمونه توزيع نمونه توزيع شود.شود.ييان مان مييشده بشده ب
Monte Carlo Monte Carlo يي موقعيت ياب موقعيت يابييها براها براParticle FilterParticle Filterاستفاده از استفاده از LocalizationLocalization.ناميده ميشود. ناميده ميشود
منابع نويز مدل حرکت و مدل سنسور مستقل از يکديگر، منابع نويز مدل حرکت و مدل سنسور مستقل از يکديگر، با ميانگين صفر در نظر گرفته ميشوند: با ميانگين صفر در نظر گرفته ميشوند:ييسفيد و گوسسفيد و گوس
با ميانگين و ماتريس با ميانگين و ماتريس يي بصورت گوس بصورت گوس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))ابد ابد يي ييمرحله انتقال ممرحله انتقال م
شود.شود.ييز در نظر گرفته مز در نظر گرفته مييانس نويز سيستم نانس نويز سيستم نييواروار
م را م را بص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ابقت دارد، در غ
شود. شود.يين بروز رسانن بروز رسانييالزم است که تخمالزم است که تخم
ع ع يي با توز با توزيير تصادفر تصادفييک متغک متغييز ز ييمانده را نمانده را نييباقباق توان در نظر گرفت: توان در نظر گرفت:يي بصورت زير م بصورت زير مييگوسگوس
تpوان آن را تpوان آن را يين و مشpاهدات من و مشpاهدات مييشpشpيين پن پييب تخمب تخميي تpرک تpرکييبرابراانس انس يpيpتpوان نسpبت وارpتpوان نسpبت وارpيين وزن را من وزن را ميpيp کpرد. ا کpرد. ايpيpجمpع وزنجمpع وزن
بهره کالمن: بهره کالمن:يي حالت حد حالت حدييبررسبررس
ستم صفرستم صفرييانس سانس سييمانده موثر و وارمانده موثر و واريي : فقط باق : فقط باق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پس ((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ک م.م.يير حرکت ربات داشته باشر حرکت ربات داشته باشييق از مسق از مسييدقدق
در اين روش خط سير نرمال بصورت ديناميک در هر حالت در اين روش خط سير نرمال بصورت ديناميک در هر حالت تغيير تغيير يي قبل قبلييبا استفاده از اطالعات حالتها و سنسورهابا استفاده از اطالعات حالتها و سنسورها
ميکند.ميکند.
حول يک خط حول يک خط يي ساز سازيي خط خطييدر اين روش در هر حالت بجادر اين روش در هر حالت بجا يي ساز سازيي خط خط سير نرمال از قبل محاسبه شده، حولسير نرمال از قبل محاسبه شده، حول
انجام ميشود.انجام ميشود.
ر نرمال مشاهدات نيز بصورت زير در نظر گرفته ر نرمال مشاهدات نيز بصورت زير در نظر گرفته ييسسشود:شود:ييمم
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
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
استفاده از چند فيلتر کالمناستفاده از چند فيلتر کالمن