1. 1 Mobile Robot Localization and Mapping using the Kalman Filter استاد درس: دکتر شيري ارائه کننده: مجيد نم نبات بنام خدا

2. 2 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

3. 3 معرفي فيلتر کالمن توسط آقاي Rudulf Emil Kalman در سال 1960 ارائه شد. شبيه HMM است. اين فيلتر يک تخمين زننده بهينه است و کمترين خطاي ميانگين مربعي را دارد. اين فيلتر يک تخمين زننده بهينه است و کمترين خطاي ميانگين مربعي را دارد. در شرايط مختلف پايدار است. در شرايط مختلف پايدار است. ميزان اطمينان هر تخمين را نيز مشخص ميکند ( واريانس ). ميزان اطمينان هر تخمين را نيز مشخص ميکند ( واريانس ).

4. 4 کاربردهای فيلتر کالمن پيگيري مسير موشکها پيگيري دست، سر و ساير اشيا استخراج حرکت لبها از ويديو در مسائل اقتصادي و ناوبري

5. 5 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

6. 6 موقعيت يابي ربات تشخيص موقعيت ربات با استفاده از يک سري اطلاعات نقشه محيط اطلاعات حرکتي ربات (Acts) مشاهدات : خروجي سنسورها (Senses). انواع مشاهدات : نسبي : انباشتگي خطا در مراحل مختلف حرکت ربات بوجود مي آيد. مثلا ادومتري نسبي : انباشتگي خطا در مراحل مختلف حرکت ربات بوجود مي آيد. مثلا ادومتري مطلق : در هر بار خطا مستقل از مشاهدات قبلي مي باشد. همچون Landmark ها

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

8. 8 مثال

9. 9 نقشه کشي ربات (Mapping) ربات بايد محيط را بگردد و ساختار آن را تعيين کند. تعيين ساختار محيط با تعيين موقعيت ربات معمولا انجام ميشود. به نقشه کشي و موقعيت يابي همزمان اصطلاحا SLAM گفته ميشود.

10. 10 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

11. 11 موقعيت يابي احتمالاتي ( اجزا ) Belief: باور و گمان ربات به بودن در يک موقعيت خاص را مشخص ميکند. به تمام موقعيتهای محيط يک احتمال نسبت داده ميشود. Belief: باور و گمان ربات به بودن در يک موقعيت خاص را مشخص ميکند. به تمام موقعيتهای محيط يک احتمال نسبت داده ميشود. Prior Belief ( تخمين پيشين ): تخمين ربات از موقعيت خود قبل از در نظر گرفتن و بررسي مشاهدات مطلق در آخرين موقعيت مي باشد. اطلاعات ربات تا کنون در حالت ايده آل در موقعيت ربات 1 و بقيه موارد 0 مي باشد.

12. 12 اجزاي مدل ( ادامه ) Posterior Belief ( تخمين پسين ): تخمين ربات از موقعيت خود بعد از در نظر گرفتن و بررسي مشاهدات مطلق در آخرين موقعيت مي باشد. State ( حالت ربات ): برداري شامل n متغير در هر زمان است که ويژگيهاي مورد نياز ربات و سيستم را توصيف ميکند. بطور مثال ميتواند شامل موقعيت و جهت ربات باشد.

13. 13 اجزاي مدل ( ادامه ) Motion Model ( مدل حرکتي ربات ): با هر فعاليت ربات موقعيت آن عوض ميشود و مدل حرکت ربات بصورت زير بيان ميشود : Sensor Model ( مدل سنسور ربات ): در هر موقعيتي سنسورها يک سري مشاهدات ميتوانند داشته باشند که بصورت زير بيان ميشود : موقعيت فعلي موقعيت قبلي حرکت (action)

14. 14 اجزا مدل ( ادامه ) Initial Belief state( تخمين موقعيت اوليه ): گمان اوليه ربات در زمان t=0 از موقعيت خودش مي باشد. انواع روشهاي موقعيت يابي بسته به تخمين اوليه : Position Tracking: موقعيت اوليه کاملا مشخص است. Global Localization: موقعيت اوليه نامشخص است و احتمال بودن در تمام موقعيتها يکسان است. Kidnapped Robot: يک موقعيت احتمال بيشتري نسبت به موقيتهاي ديگر دارد.

15. 15 الگوريتم موقعيت يابي موقعيت يابي بصورت يک مساله تخمين Bayesian مدل ميشود. در هر حالت ربات گمان حالت قبل خود را با استفاده از مشاهدات نسبي و قطعي خود بروز رساني ميکند. براي سادگي مدل حرکت را بصورت مدل مارکو در نظر ميگيريم : با استفاده از تئوري Total Probability و قانون بيز بدست مي آوريم :

16. 16 الگوريتم موقعيت يابي ( ادامه ) طبق معادله بالا گمان پيشين بودن در برابر با احتمال آمدن از به ضربدر احتمال بودن در حالت با توجه به مشاهدات قبلي مي باشد. براي بدست آوردن تخمين پسين از فرض مارکو استفاده ميکنيم : با استفاده از قانون بيز، مقدار تخمين پسين بصورت زير بدست مي آيد : يک ثابت نرماليزه کننده برابر با مي باشد. يک ثابت نرماليزه کننده برابر با مي باشد.

17. 17 انواع Belief دو نوع گسسته و پيوسته وجود دارد. در نوع گسسته لازم است که فضاي موقعيت گسسته سازي شود. روشهاي مختلفي براي گسسته سازي وجود دارد : گرافهاي مکاني : گسسته سازي با محدود کردن مجموعه فعاليتهاي ربات انجام ميشود. شبکه بندي : فضاي موقعيت را با يک دقت خاص شبکه بندي ميکنند. Particle Filter: گمان با استفاده از مجموع وزني m نمونه توزيع شده بيان ميشود. استفاده از Particle Filter ها براي موقعيت يابي Monte Carlo Localization ناميده ميشود.

18. 18 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

19. 19 فيلترهاي کالمن تابع Belief را بصورت گوسي فرض ميکنند. توزيع گوسي 1 بعدي توزيع گوسي n بعدي

20. 20 فرضهاي مدل LKF فيلترهاي کالمن خطي فرض ميکنند ديناميک سيستم خطي باشد. منظور از خطي بودن، خطي بودن معادلات مدل حرکت و سنسور مي باشد. مدل حرکت مدل سنسور چرا فيلتر مي نامند؟ A ماتريسي است که حالت فعلي را به قبلي مرتبط ميکند. ماتريس H مشاهدات سنسورهاي ربات را در حالتهاي مختلف بدون در نظر گرفتن نويز نشان ميدهد. به اين دليل که مشاهدات نويز دار را دريافت ميکند و سپس سعي ميکند موقعيت صحيح را تخمين بزند به آن فيلتر ميگويند.

21. 21 فرضهاي مدل LKF( ادامه ) منابع نويز مدل حرکت و مدل سنسور مستقل از يکديگر، سفيد و گوسي با ميانگين صفر در نظر گرفته ميشوند : هر Belief بصورت گوسي با ميانگين و ماتريس کوواريانس توزيع گوسي مشخص ميشود. بدليل در نظر گرفتن توزيع گوسي فيلترهاي کالمن Unimodel مي باشند و لازم است که در ابتدا موقعيت اوليه ربات مشخص باشد. بدليل در نظر گرفتن توزيع گوسي فيلترهاي کالمن Unimodel مي باشند و لازم است که در ابتدا موقعيت اوليه ربات مشخص باشد. ميزان اطمينان تخمين موقعيت تخمين زده شده

22. 22 مثال فرض کنيد رباتي است که در يک جهت با سرعت ثابت s حرکت کند. مدل سيستم ربات برابر است با : نويز مدل سيستم توزيع گوسي با ميانگين صفر و واريانس در نظر گرفته ميشود. مدل سنسور که براي مدل کردن خروجيهاي ممکن سنسورها در يک حالت خاص بکار ميرود را نيز بصورت زير در نظر ميگيريم : تخمين اوليه موقعيت را نيز با واريانس در نظر ميگيريم.

23. 23 مثال ( ادامه ) ما مقدار نويز را نمي دانيم، لذا فقط مدل تقريبي حرکت و سنسور را با در نظر گرفتن نويز صفر ميتوانيم داشته باشيم : اکنون اگر ربات از محل اوليه يک گام زماني حرکت کند، موقعيت ربات تقريبا برابر با مقدار زير ميشود : ميزان دقت تخمين برابر است با : ميزان دقت تخمين برابر است با :

24. 24 مثال ( ادامه ) بعد از پيشگويي موقعيت جديد، اکنون لازم است که خروجي سنسورها خوانده شود و با توجه به مقادير مشاهدات موقعيت پيشگويي شده اصلاح شود. لازم است که مقادير مشاهدات را بگونه اي با مدل تخميني از موقعيت ترکيب کنيم. براي اينکار از معيار وزني ميتوان استفاده کرد : لازم است که مقادير مشاهدات را بگونه اي با مدل تخميني از موقعيت ترکيب کنيم. براي اينکار از معيار وزني ميتوان استفاده کرد :

25. 25 ( مثال ( ادامه فرمول بالا را ميتوان بصورت زير بازنويسي کرد : بررسي رفتار K جالب ميباشد ! KF در دو مرحله کار ميکند : پيشگويي، اصلاح. ابتدا با مدل سيستم موقعيت را پيشگويي ميکند و سپس با مدل سنسور موقعيت پيشگويي شده را اصلاح ميکند.

26. 26 مثال کشتي و ستاره مرحله پيشگويي مرحله اصلاح

27. 27 مدل گوسي سيستم و سنسور براي محاسبه Belief لازم است که و محاسبه شوند. براي اينکار از مدل سيستم و مدل سنسور استفاده ميکنيم. ضريب را ميتوان بصورت يک توزيع گوسي با ميانگين و واريانس صفر در نظر گرفت. لذا بدست مي آوريم : بطور مشابه بدست مي آوريم :

28. 28 تخمين پيشين ( گام پيشگويي ) Prior Estimation Error

29. 29 تخمين پسين ( گام اصلاح ) مراحل کار : بردن دو توزيع گوسي از فضاهاي متفاوت به يک فضا ترکيب دو توزيع گوسي ساده سازي بهره کالمن (Kalman Gain)

30. 30 بررسي معادلات در مرحله پيشگويي واريانس مرحله قبل به اين مرحله انتقال مي يابد (Propagate) و همچنين واريانس نويز سيستم نيز در نظر گرفته ميشود.

31. 31 بررسي معادلات ( ادامه ) فيلتر کالمن انتظار دارد که مشاهدات مطابق با موقعيت تخميني با درنظر گرفتن نويز صفر باشد : را ميتوان بصورت يک متغير تصادفي با توزيع گوسي زير در نظر گرفت : را ميتوان بصورت يک متغير تصادفي با توزيع گوسي زير در نظر گرفت :

32. 32 بررسي معادلات ( ادامه ) ايده آل اين است که تفاوت ميان مشاهدات سنسورها و خروجي مورد نظر صفر باشد. اگر مانده صفر باشد يعني نيازي به بروز رساني تخمين پيشين نمي باشد و تخمين پيشين با مشاهدات کاملا مطابقت دارد، در غيراينصورت لازم است که تخمين بروز رساني شود. باقيمانده را نيز يک متغير تصادفي با توزيع گوسي بصورت زير مي توان در نظر گرفت : خطاي باقيمانده (Residual)

33. 33 بررسي معادلات ( ادامه ) واريانس باقيمانده ميتواند به عنوان ميزان خطا و نويز خروجي سنسورها مورد استفاده قرار بگيرد. براي ترکيب تخمين پيشين و مشاهدات ميتوان آن را جمع وزني کرد. اين وزن را ميتوان نسبت واريانس اين دو تعريف کرد. در اين حالت واريانس هر يک کمتر باشد سهم بيشتري در تخمين نهايي خواهد داشت. نقش ؟ بردن واريانس تخمين اوليه از فضاي حالت به فضاي مشاهدات

34. 34 بررسي معادلات ( ادامه ) بررسي حالت حدي بهره کالمن : نويز مشاهدات = 0 : فقط باقيمانده موثر و واريانس سيستم صفر واريانس تخمين اوليه = 0 : ؟ بهره کالمن (KG) قلب فيلتر کالمن ناميده ميشود وثابت ميشود که KF بدليل استفاده از KG يک تخمين زننده بهينه براي يک سيستم با ديناميک خطي مي باشند و تخمين با کمترين واريانس ممکن را مي زند. بهره کالمن (KG) قلب فيلتر کالمن ناميده ميشود وثابت ميشود که KF بدليل استفاده از KG يک تخمين زننده بهينه براي يک سيستم با ديناميک خطي مي باشند و تخمين با کمترين واريانس ممکن را مي زند.

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

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

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

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

39. 39 State Estimation Error vs 3s Region of Confidence

40. 40 Sensor Residual vs 3s Region of Confidence بصورت تجربي ثابت ميشود که براي جلوگيري از انحراف سيستم لازم است که مانده همواره بايد در يک محدوده اطميناني باشد.

41. 41 مثال 2 شبيه مثال قبل است، فقط اينبار موقعيت اوليه حرکت ربات را اشتباه به سيستم مي دهيم :

42. 42 مشکلات بعضي سنسورها همچون انکدرها و ادومتري داراي انباشتگي خطا هستند و لازم است که جز مدل سيستم محسوب شوند. راه حل : لازم است که جملاتي به مدل سيستم اضافه شود و معادلات دوباره بدست بيايند. معادلات شبيه معادلات قبل مي باشند و فقط جملاتي به آنها اضافه ميشود. بيشتر mobile robot ها را بصورت خطي نمي توان مدل کرد. خطي سازي مدل ديناميک سيستم (PKF) گسترش مدل LKF به سيستمهاي غير خطي (EKF).

43. 43 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

44. 44 Perturbation Kalman Filter مدل سيستم وسنسور در حالت غير خطي ميتوانن بصورت زير در نظر گرفته شوند : براي خطي سازي مدل سيستم يک خط سير نرمال (nominal trajectory) تعريف ميشود و سپس حول اين خط سير سير حرکت ربات با استفاده از تقريب درجه اول بسط تيلور خطي ميشود.

45. 45 PKF( ادامه ) براي خطي سازي مدل سيستم، معادله غير خطي مربوط به حالت را به يک ترکيب خطي از خط سير نرمال و يک جز آشفتگي تجزيه ميشود. روش محاسبه خط سير نرمال بصورت زير مي باشد : براي محاسبه جز آشفتگي بصورت زير عمل ميکنيم :

46. 46 PKF( ادامه ) h.o.t. ؟ ماتريس A ماتريسي n*n از مشتقات جزئي f مي باشد.

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

48. 48 بررسي روش PKF امکان محاسبه ماتريس A,F قبل از راه اندازي ربات وجود دارد که ميتواند منجر به افزايش سرعت محاسبات در حين حرکت ربات شود. خط سير نرمال در نظر گرفته شده در ابتداي کار بايد دقيق باشد وگرنه خطاي انباشتگي زياد ميشود و ديگر جملات درجه بالا سري تيلور قابل چشمپوشي نمي باشند. براي طراحي دقيق خط سير نرمال در ابتداي کار لازم است که اطلاعات دقيق از مسير حرکت ربات داشته باشيم.

49. 49 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

50. 50 Extended Kalman Filter در اين روش خط سير نرمال بصورت ديناميک در هر حالت با استفاده از اطلاعات حالتها و سنسورهاي قبلي تغيير ميکند. در اين روش در هر حالت بجاي خطي سازي حول يک خط سير نرمال از قبل محاسبه شده، حول خطي سازي انجام ميشود. سير نرمال مشاهدات نيز بصورت زير در نظر گرفته ميشود :

51. 51 EKF( ادامه ) با در نظر گرفتن خط سير نرمال بصورت بالا جز آشفتگي در مراحل پيشگويي و اصلاح همواره صفر ميشود و فرمولهاي زير مشابه LKF بدست مي آيند. گام اصلاح : گام پيشگويي :

52. 52 EKF( ادامه ) F,H ماتريسهاي ژاکوبين هستند که اينبار در هر حالت لازم است که محاسبه شوند :

53. 53 استفاده از EKF برای موقعيت يابی

54. 54 مدل يک Mobile Robot R X Y  x y G v R از ديد ربات معادلات حرکت بصورت زير می باشند : از نظر مختصات کلی حرکت بصورت زير ميتواند مدل شود : مدل سيستم با در نظر گرفتن نويز بصورت زير ميتواند مدل شود :

55. 55 مدل يک Mobile Robot( ادامه ) اکنون لازم است که مدل سيستم را با استفاده از EKF خطی سازی کنيم : برای خطی سازی سيستم لازم است که برای زوايای کوچک فرضهای زير در نظر گرفته شود.

56. 56 مدل يک Mobile Robot( ادامه ) مدل سيستم در انتها بصورت زير در می آيد : با توجه به مدل بالا، واريانس پيشين مقدار زير ميشود :

57. 57 مدل سنسور برای يک Mobile Robot از ديد ربات از نظر مختصات عمومی اين مدل نيز لازم است که خطی سازی شود. R X Y x y G L z

58. 58 مدل سنسور برای يک Mobile Robot ( ادامه ) بعد از محاسبه مدل زير بدست می آيد :

59. 59 EKF for SLAM بردار حالت اينبار علاوه بر شامل شدن مختصات ربات شامل مختصات علايم نيز می باشد. بردار کوواريانس نيز بصورت زير در نظر گرفته ميشود : مدل سنسور بصورت زير در می آيد :

60. 60 EKF for SLAM

61. 61 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

62. 62 بهبودها Iterated EKF: در اين روش محاسبه تخمين پسين چندين بار انجام ميشود. بدست آوردن تخمين پسين مبتني بر تخمين پسيين تکرار قبل موجب افزايش دقت تخمين پسين ميشود. GMM: ميتوان بجاي اينکه گمان را بصورت يک توزيع گوسي در نظر بگيريم بصورت GMM در نظر بگيريم. استفاده از چندين کالمن فيلتر براي بررسي داده ها با فرضيه های مختلف

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

64. 64 فهرست معرفي موقعيت يابی ربات موقعيت يابی احتمالاتی فيلترهای کالمن (LKF) Perturbation Kalman Filter Extended Kalman Filter بهبودها مراجع

65. 65 مراجع [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.htmlhttp://www.cs.unc.edu/~weltch/kalmanlinks.html [4] http://www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html http://www.cs.unc.edu/~tracker/ref/s2001/kalman/index.html

66. 66

67. 67 پيوست 1 : يکپارچه سازي با مدل سنسور قانون بيز را اعمال ميکنيم : با فرض مارکو داريم :

68. 68 پيوست 1 : يکپارچه سازي با مدل سنسور ( ادامه ) مخرج صرفا يک نرماليزه کننده است که با فرمول زير محاسبه ميشود :

69. 69 پيوست 2 : تخمين پسين KF

70. 70 پيوست 2 : تخمين پسين KF ( ادامه )

71. 71