1. دکتر سعید شیری قیداری & فصل 5 کتاب
Localization
دکتر سعید شیری قیداری
& فصل 5 کتاب
Amirkabir University of Technology Computer Engineering & Information Technology Department

2. مقدمه قابلیتNavigation یکی از نیازمندیهای مهم روبات متحرک است.
برای موفقیت در این امر میبایست تمام چهار جزء اصلی آن نیز بخوبی انجام شوند:
: Preceptionروبات میبایست تعبیر مناسبی از داده های سنسورها داشته باشد
: localizationروبات میبایست موقعیت خودش را در محیط پیدا نماید
: Cognition روبات میبایست در مورد نحوه رسیدن به هدف تصمیم گیری نماید.
: Motion Controlروبات میبایست خروجی موتورهایش را برای حرکت در مسیر مطلوب کنترل نماید.

3. مشکل اصلی بر سر راه مکان یابی
مسئله مکان یابی بسی فراتر از مسئله پیدا کردن موقعیت یک روبات در محیط است. ) این مسئله با GPS قابل حل نیست(
روبات ممکن است نیازمند دانستن فاصله نسبی اش تا اشیا و آدمی باشد
روبات ممکن است نیاز به داشتن نقشه ای از محیط باشد تا برای انجام کارهایش قادر به planning باشد.
مسئله مکان یابی بشدت وابسته به استفاده از سنسورهای روبات است. اما به خاطر اینکه این سنسورها دارای عدم قطعیت هستند مسئله مکان یابی دقیق روبات به مسئله ای پیچیده تبدیل میشود.

4. مکان یابی

5. نویز سنسورها مثالهائی از نویز سنسورها:
تغییر روشنائی محیط میتواند به تغییر مقدار Hue در تصویر منجر شود که بصورت نویز تصادفی در تصویر جلوه خواهد نمود.
وقتی که یک پالس سونار به سمت یک سطح صیقلی زاویه دار فرستاده شود، قسمت اعظم آن منعکس شده و آن بخشی از سیگنال که باز میگردد ممکن است هر از چندگاهی قابل تشخیص بوده و در نتیجه یک محیط ثابت از نظر روبات متغیر به نظر برسد

6. مدل خطا برای تخمین موقعیت با استفاده از اودومتری
موقعیت یک روبات بصورت زیر نشان داد ه میشود:
برای یک روبات دیفرانسیلی اگر در فواصل منظم از x,y,q نمونه برداری شود خواهیم داشت:
فاصله بین چرخها
فاصله طی شده توسط چرخها

7. مدل خطا برای تخمین موقعیت با استفاده از اودومتری
فرمول فوق فقط میتواند یک تقریب کلی از موقعیت روبات بدست دهد زیرا خطاهای در طول زمان با هم جمع خواهند شد.
برای محاسبه ماتریس کواریانس خطا اگر فرض شود که این ماتریس در ابتدا معلوم است خواهیم داشت:
فاصله طی شده توسط هر چرخ
ثوابت خطا

8. مدل خطا برای تخمین موقعیت با استفاده از اودومتری
اگر مقدار p و مقدار حرکت چرخها کارولیشن نداشته و f را بتوان با یک بسط تیلور مرتبه اول تقریب زد قانون انتشار خطا بصورت زیر خواهد بود:

9. انتشار خطا برای حرکت در روی یک خط

10. دو روش ممکن در روباتیک
نقشه شکل زیر را در نظر بگیرید. روبات برای انجام Navigation نیاز دارد که از سنسورها برای جلوگیری از برخورد با اشیا استفاده نماید. همچنین روبات باید بتواند موتورهایش را کنترل نماید.
آیا روبات برای رفتن از اتاقی به اتاق دیگر نیاز به مکان یابی دارد؟

11. مثال

12. استفاده از نقشه
ممکن است مکان یابی برای جابجائی بین دو اتاق ضروری باشد. روبات باید بتواند موقعیت خود را در روی نقشه پیدا کند تا از رسیدن به هدف مطمئن گردد.
همچنین روبات باید بتواند هدف را در روی نقشه مشخص نماید.

13. پرهیز از ایجاد نقشه
مکتب Behaviour-based از طرف دیگر معتقد است که بعلت وجود نویز در سنسورها باید از ایجاد نقشه ژئومتریک پرهیز نمود.
در این روش از ارائه موقعیت آشکار و همچنین طرح ریزی مسیر پرهیز میشود.
در این روش اعتقاد بر این است که میتوان فرایندی برای انجام یک navigation مشخص انجام داد. برای مثال برای رفتن از اتاق A به اتاقB میتوان از یک رفتار برای دنبال کردن دیوار و از رفتار دیگری برای تشخیص اتاق مقصد که با فرش قرمز مفروش شده است استفاده نمود

14. ساختار Behavior based navigation

15. ساختار Behavior based navigation
از مزایای این روش سادگی پیاده سازی برای یک مسئه با اهداف محدود است.
از معایب آن این است که بسادگی برای سایر مسایل و یا محیط های بزرگترقابل تعمیم نیست.
همچنین طراحی رفتارهائی مانند تعقیب یک دیوار بسادگی قابل پیاده سازی نبوده و به سخت افزار روبات بستگی خواهد داشت.
در عمل ممکن است رفتارهای متعددی بطور همزمان فعال باشند که انتقال بین آنها به تنظیمات دقیقی نیاز دارد که وقتی رفتاری جدید به سیستم اضافه میشود همه این تنظیمات به هم میخورد.

16. ساختار model-based navigation
در این روش
روبات با استفاده از سنسورها اطلاعاتی در مورد محیط جمع میکند
سپس باوری در مورد موقعیت روبات در روی نقشه بوجود میآید.
در این روش همکاری بین انسان و روبات ساده تر انجام میشود که نقشه این محیط را میسازد.
این خطر وجود دارد که تصور روبات از محیط و واقعیت تفاوت زیادی داشته باشند.
امید میرود که در آینده بتوان روباتهائی ساخت که قادر به تهیه نقشه و حرکت در محیط های مختلفی باشند.

17. ساختار model-based navigation

18. نمایش نقشه
روبات باید نقشه ای برای نمایش و یا مدل کردن محیط ایجاد نماید. در اینصورت:
نقشه باید چه جنبه هائی از محیط را شامل شود؟
وبالعکس نقشه در چه سطحی قادر به نمایش محیط است؟
همچنین روبات باید باوری نسبت به موقعیت خود در روی نقشه داشته باشد.) ممکن است روبات موقعیت خودش را با یک و یا چندین نقطه در روی نقشه مشخص نماید(

19. انواع مختلف نمایش موقعیت روبات در روی نقشه
a) Continuous map
with single hypothesis
b) Continuous map
with multiple hypothesis
c) Discretized map
with probability distribution
d) Discretized topological
map with probability
distribution

20. باور تک فرضیه ای
مستقیم ترین روش نشان دادن موقعیت روبات در یک نقطه باور تک فرضیه ای است که در آن فقط یک موقعیت برای روبات متصور میشود.
حسن این روش عدم وجود ابهام درموقعیت است که باعث میشود تا تصمیم گیری در مواردی نظیر طراحی مسیر بسادگی قابل انجام باشد.
به همین ترتیب به روز کردن باور روبات به موقعیتش نیز میتواند بسادگی انجام پذیرد. اما وجود خطا در سنسورها این امر را با چالش زیادی روبرو میسازد.

21. باور تک فرضیه ای
Single-hypothesis Belief – Continuous Line-Map

22. باور تک فرضیه ای
Single-hypothesis Belief – Grid and Topological Map

23. باور چند فرضیه ای
در این روش روبات بطور همزمان چندین رد از موقعیت را دنبال میکند.
ممکن است نظم وترتیبی به موقعیت ها داده شود بنحویکه موقعیتی محتملتر از سایرین باشد.
برای مثال ممکن است موقعیت روبات در صفحه دو بعدی بصورت یک توزیع احتمال گوسی با مقدار میانگین و واریانس نشان داده شود.
در این روش روبات بطور مشخص عدم قطعیت در موقعیت خودش را نگهداری خواهد نمود.
از معایب این روش مشکل بودن تصمیم گیری است
از لحلظ محاسباتی تصمیم گیری ها پر هزینه هستند.

24. نمایش نقشه نحوه نمایش نقشه مرتبط با نحوه نمایش موقعیت روبات است.
محدودیت های نقشه:
دقت نقشه باید با دقتی که روبات برای رسیدن به هدف لازم دارد منطبق باشد
دقت نقشه و نوع ویژگی های آن باید با دقت سنسورها و ویژگی هائی که روبات قادر به استخراج آن است سازگار باشد.
پیچیدگی های نقشه تاثیر مستقیمی بر پیچیدگی عملیات وابسته به آن نظیر mapping, localization , navigation دارد.
انتخاب نحوه نمایش نقشه علاوه بر محدودیت های فوق به صورت مسئله نیز ارتباط خواهد داشت

25. نحوه نمایش باور نسبت به موقعیت
پیوسته:
گسسته:
دقت آن توسط سنسور تعیین میشود
دقت آن بستگی به نحوه گسسته سازی دارد
معمولا چندین فرضیه ارائه میشود
معمولا تک فرضیه ای است
توان پردازشی بالائی نیاز دارد
بسادگی قابل محاسبه است

26. نمایش محیط نمایش محیط مدل کردن محیط Continuos Metric x,y,q
Discrete Metric metric grid
Discrete Topological  topological grid
مدل کردن محیط
Raw sensor data, e.g. laser range data, grayscale images
large volume of data, low distinctiveness on the level of individual values
makes use of all acquired information
Low level features, e.g. line other geometric features
medium volume of data, average distinctiveness
filters out the useful information, still ambiguities
High level features, e.g. doors, a car, the Eiffel tower
low volume of data, high distinctiveness
filters out the useful information, few/no ambiguities, not enough information

27. نمایش پیوسته نقشه geometric map
در این روش ویژگی های محیط با مقادیر پیوسته بدقت نشان داده میشوند. معمولا از این روش فقط در نقشه های 2 بعدی که پیچیدگی محاسباتی کمتری دارند استفاده میشود.
در این روش فرض میشود که هر شی موجود در محیط در نقشه نیز وجود خواهد داشت.
ممکن است برای سادگی کار فقط اشیائی را در نقشه ذکر کرد که روبات قادر به ادراک آنها از طریق سنسورهایش میباشد.
در این نقشه ها فقط موقعیت فیزیکی اشیا ذکر میشود و سایر ویژگی ها نظیر رنگ، بافت وغیره حذف میشوند.
همچنین برای سادگی همه اشیا با چند ضلعی های گوژ نمایش داده میشوند.

28. نمایش محیط با استفاده از خط
در روبات های خانگی که از لیزر استفاده میشود میتوان خطوط مربوط به محیط را از روی نقاط اندازه گیری شده استخراج نمود.
در این حالت میتوان نقشه را با خطوطی با موقعیت و جهت دلخواه نشان داد.
برای چنین نقشه ای میتوان موقعیت روبات را با هر یک از روشهای ذکر شده قبلی نشان داد.

29. نمایش محیط با استفاده از خط
Key advantage is potentially high accuracy

30. Cell decomposition روشی است برای ساده تر کردن نمایش محیط
در این روش محیط به نواحی تجزیه میشود که فضای آزاد ویا free space نامیده میشوند.
میتوان بجای ذخیره کل محیط فقط گره های مربوط به نواحی آزاد را بصورت یک گراف ذخیره نمود.
ایده اصلی این روش این است که موقعیت محلی روبات در هر ناحیه آزاد چندان مهم نیست. بلکه آنچه که اهمیت دارد توانائی روبات در در حرکت از یک ناحیه به ناحیه آزاد مجاورش است.

31. Cell decomposition
ممکن است اندازه سلولهای بکار رفته ثابت و یا متغیر باشد.

32. Cell decomposition
در عمل تجزیه دقیق محیط بسیار مشکل است زیرا جمع آوری اطلاعات آن کار سختی است. در نتیجه بجای یک نقشه پیوسته از نقشه دیجیتایز شده استفاده میشود.

33. Adaptive cell decomposition

34. Occupancy grid
در این روش محیط توسط یک شبکه گسسته نمایش داده میشود که در آن هر سلول یا توسط یک مانع اشغال شده است و یا اینکه خالی بوده و فضای آزاد بحساب میاید.
این روش بخصوص در مواقعی که روبات مجهز به سنسورهای فاصله نظیر سونار است استفاده میشود زیرا به روز کردن نقشه ساده تر است.
در این روش هر سلول با یک شمارنده همراه است که مقدار آن در مواقعی که سنسور فاصله برای آن مقدار برمیگرداند افزایش میابد و در مواقعی که اندازه گیری از آن رد میشود کاهش میابد.
از معایب این روش نیاز به داشتن حافظه بزرگ است.

35. Topological map
در این روش از اندازه گیری مقادیر هندسی محیط پرهیز میشود در عوض بر مشخصاتی از محیط که برای مکان یابی روبات مهم است تاکید میشود.
در حالت کلی یک نقشه توپولوژیکال یک گراف است که گره ها و لینک های اتصال بین انها را نشان میدهد.
گره ها مکان های موجود در محیط و لینک ها مجاورت هر زوج از گره ها را نشان میدهد.
وقتی که یک لینک دو نقطه را به هم متصل میکند به این معناست که روبات میتواند مستقیما از یک محل به محل دیگری برود .

36. Topological map
برای اینکه روبات بتواند با استفاده از یک نقشه توپولوژیک جابجا شود باید:
بتواند موقعیت خودش را برحسب گره ها ی نقشه پیدا نماید
درثانی روبات باید بتواند قادر به حرکت بین گره ها باشد.
اندازه گره ها و ابعاد آن باید بهینه شوند تا با توانائی های سخت افزاری روبات هماهنگی داشته باشند.
انگیزه اصلی در استفاده از نقشه های توپولوژیکی این است که محیط ممکن است ویژگی های غیرهندسی مهمی داشته باشد که روبات بتواند آنها را تشخیص دهد.
ممکن است landmark هائی در محیط قرار داده شوند که روبات با تشخیص آنها بتواند محیط خودش را تشخیص دهد.

37. Topological map

38. Topological map

39. Topological map

40. مشکلات تهیه نقشه یک محیط واقعی
محیط واقعی یک محیط پویاست که در آن اشیا بسیاری در حال حرکت هستند.
یکی از نقاط ضعف روشهای فوق قابلیت جدا کردن اشیا ثابت از اشیا متحرک است.
استفاده از بینائی روبات میتواند برای تشخیص اشیا ثابت از متحرک مفید واقع شود.
ساخت نقشه های توپو لوژیکی برای محیط های خارج از اتاق مشکل است.
در عمل ناگزیر به sensor fusion هستیم.

41. مثالی از مکان یابی بر پایه احتمالات
فرض کنید که روبات میخواهد با استفاده از اندازه گیری S موانع را تشخیص دهد:
What is p(obstacle|s)?

42. مثالی از مکان یابی بر پایه احتمالات
عمل اندازه گیری p(obstacle|s) مشکل است.
با توجه به اینکه مدلی برای سنسور در اختیار میباشد، اغلب ساده تر است که مقدار زیر محاسبه شود:p(s|obstacle)
با اعمال قانون بیز خواهیمداشت:

43. نرمالیزه کردن

44. مثال عددی
فرض کنید که روبات میخواهد با استفاده از اندازه گیری S موانع را تشخیص دهیم:
What is p(obstacle|detection) if
p(obstacle) = 0.1
p(detection|obstacle)=0.9
p(detection|no obstacle)=0.05

45. مکان یابی بر اساس نقشه احتمالاتی Probabilistic, Map-Based Localization
این روشها موقعیت روبات را با یک تابع احتمال نشان میدهند.
دو تکنیک اساسی در این زمینه عبارتند از:
روش مارکف: یک توزیع احتمال برای همه مکانهای ممکن برای روبات بدست میاورد
روش کالمن فیلتر: یک توزیع گاوسی برای موقعیت روبات بدست آورده و بدنبال محلی که با آن تطبیق نماید میگردد.

46. مکان یابی بر اساس نقشه احتمالاتی Probabilistic, Map-Based Localization
یک روبات متحرک را در محیطی شناحته شده در نظر بگیرید.
این روبات از یک محل اولیه شروع به حرکت نموده و با استفاده از Odometery موقعیت خود را مشخص مینماید.
روبات بعد از پیمودن مسافتی دچار عدم قطعیت در مورد موقعیت خود خواهد شد.
روبات میتواند موقعیت خود را با انجام مشاهداتی )با استفاده از سنسورها( از محیط به روز نماید
هر مشاهده منجر به بک تخمین از موقعیت روبات میشود که میتوان آنرا با موقعیت حاصل از Odometery ترکیب نموده و بهترین تخمین از محل واقعی روبات را بدست آورد.

47. مکان یابی بر اساس نقشه احتمالاتی Probabilistic, Map-Based Localization
عمل مکان یابی در دو مرحله انجام میشود:
Action update
مدلی برای نحوه عمل روبات فرض میشود که با استفاده ازقرائت انکدرها Ot و بر مبنای باور موجود نسبت به وضعیت یعنی St-1 باور جدیدی نسبت به وضعیت روبات بدست میآید
.S’t=Act(Ot , St-1 )
Perception update
مدلی از ادراک به ورودیهای سنسور it و باور به روز شده S’t اعمال میشود تا یک باور تصحیح شده از وضعیت فعلی روبات بدست آید.
St=See(it , St )

48. مکان یابی بر اساس نقشه احتمالاتی Probabilistic, Map-Based Localization
درحالت کلی درمرحله به روز کردن actionعدم قطعیت باورروبات نسبت به موقعیت خود افزوده خواهد شد
اما درمرحله به روز کردن perception باعث تصحیح باور نسبت به وضعیت میگردد.
در روش مارکف برای هر وضعیت ممکن روبات یک تابع احتمال جداگانه منظور و با انجام مشاهدات همه آنها به روز میشوند.
در روش کالمن فقط از یک توزیع احتمال گوسی برای توصیف موقعیت روبات استفاده میشود که با یک مقدار میانگین ویک واریانس بیان میشود. در هر مرحله مقدار این دو متغیر به روزخواهند شد.

49. Improving belief state by moving

50. مراحل Map-Based Localization

51. مقایسه روشهای مارکف وکالمن
Kalman filter localization
tracks the robot and is inherently very precise and efficient.
However, if the uncertainty of the
robot becomes to large (e.g. collision with an object) the Kalman filter will fail and the
position is definitively lost.
Markov localization
localization starting from any
unknown position
recovers from ambiguous
situation.
However, to update the probability of all positions within the whole state space at any time requires a
discrete representation of the
space (grid). The required memory and calculation power can thus become very important if a fine
grid is used.

52. مکان یابی به روش مارکف
در این روش باور روبات نسبت به موقعیت توسط تابع توزیع احتمال دلخواه نشان داده میشود.
فضای موقعیت روبات با استفاده از یک grid و یا یک گراف توپولوژیک به تعدادی موقعیت محدود تجزیه میشود. ( صدها و گاها میلیونها )
در هر مرحله از بروز رسانی موقعیت تمامی نقاط فضای وضعیت به روز میشوند. اینکار با استفاده از ابزارهای تئوری احتمال انجام میشود.

53. مکان یابی به روش مارکف
احتمال اولیه حضور روبات r در لحظه t در موقعیت اولیه l با P(A) و یا با P(rt=l) نشان داده میشود. این احتمال مستقل از هر دانشی است که وجود دارد.
در عمل مایلیم احتمال مربوط به هرموقعیت روبات را بااستفاده ازداده های سنسور هامحاسبه کنیم. برای اینکار از احتمال شرطی P(rt=l | it) استفاده میشود.
فرمول بیز

54. مکان یابی به روش مارکف به روز کردن تابع See
p(l): belief state before perceptual update process
p(i |l): probability to get measurement i when being at position l
consult robots map, identify the probability of a certain sensor reading for each possible position in the map
p(i): normalization factor so that sum over all l for L equals 1.

55. مکان یابی به روش مارکف به روز کردن تابع Act
برای محاسبه باورجدید روبات به بودن در یک وضعیت باید احتمال رسیدن به آن وضعیت از تمامی وضعیتهای ممکن بررسی و انتگرال گیری شود.
بعبارت دیگر مجموع احتمال یک موقعیت l با استفاده از تمامی موقعیتهای l’ قبلی و مشاهده انکدر o ساخته میشود.

56. فرض اساسی روش مارکف
روش مارکف بر پایه این فرض اساسی است که موقعیت فعلی روبات فقط به موقعیت قبلی و مقادیر حاصل ازاندازه گیری و یا ادراک بستگی دارد.
در حالت کلی ممکن است این فرض صحیح نباشد مثلا مقدار قرائت سنسورها ممکن است تابعی ازtrajectory باشند و از اینرو نیازمند در نظر گرفتن تاریخچه های موقعیت های قبلی باشند.
با این وجود فرض مارکف باعث میشود تا محاسبات مربوطه به سادگی انجام پذیرند.

57. مثالی از مکان یابی به روش مارکف با استفاده از یک نقشه توپولوژیکی
در یک نقشه توپولوژیکی محیط به نقاط مجزائی تجزیه شده است از اینرو بستر استفاده از روش مارکف ازقبل آماده است.
در این مثال روباتی با نام درویش با استفاده از نقشه توپولوژیک دانشکده که در آن راهروها وکلاسها توسط نقاطی در روی نقشه مشخص شده اند و همچنین استفاده از سنسورها از محل مبدا به مقصد مورد نظر حرکت میکند. نقشه ممکن است اتصالات غلطی را نیز داشته باشد.
این روبات توانست با استفاده از روش مارکف با موفقیت در مسابقات 1994انجمن هوش مصنوعی امریکا برنده شود.

58. مثال
روبات فاقد انکدر بوده و با استفاده از رینگ اولترا سونیک عمل مکان یابی و ممانعت از برخورد با موانع را انجام میدهد.
روبات با استفاده از سونار سعی در ادراک و تشخیص عبور از محل اتصال سالن ها می نمود.

59. نقشه توپولوژیک
محیط شامل فضاهای مستطیلی و اشیا متداول در محیط های کار است.
نقشه هندسی
در یک نقشه توپولوژیکی مهمترین مسئله تصمیم گیری شامل تعیین گره ها و اتصال بین آنها میباشد.
نقشه توپولوژیکی

60. مدل ادراک
روبات با استفاده از سنسورهای سمت چپ و راست خود محل اتصال بین اتاقها و سالنها را تشخیص میدهد.
در سیستم ادراکی فقط از زمان برای تشخیص وقایع استفاده میشود. برای مثال وقتی روبات برای مدت 1 ثانیه در فاصله 7 تا 17 سانتیمتری خود بطور پیوسته در دیوار سالن مانعی را تشخیص میدهد رویداد “ در بسته ” تشخیص داده میشود.

61. باور موقعیت
برای نمایش باور موقعیت به هر گره توپولوژیکی n مقدار احتمال بودن در محدوده آن اختصاص داده می شود . P(rt=n)
به روز کردن مقدار این باور وقتی انجام میشود که یک ویژگی سطح بالا نظیر راهرو و یا تقاطع استخراج میشود. معمولا این محاسبه بصورت تقریبی و بر اساس liklihood صورت میپذیرد.
p(n¦i)= p(i¦n) p(n)
p(n¦i): new likelihood for being in position n
p(n): current believe state
p(i¦n): probability of seeing i in n

62. استفاده از جدول p(i¦n): probability of seeing i in n
سیستم استخراج ویژگی درویش قادر است تا فقط 4 نوع ویژگی را تشخیص دهد. و از آنجائیکه هر گره دارای 5 حالت است لذا در مجموع میتوان با استفاده از یک جدول و یا certainty matrixتخمینی از مقدار احتمال p(i¦n) را بیان نمود.

63. به روز رسانی action
از آنجائیکه درویش فاقد انکدر است، و رویدادهای ادراکی بصورت غیرهمزمان رخ میدهند لذا عمل به روز رسانی مربطه به قسمت action انجام نمیشود.
با این وجود در عمل با هر رویداد ادارکی ترکیبی از به روز رسانی action و perception بطور همزمان انجام میشود.
Ø t-i is used instead of t-1 because the topological distance between n’ and n can very depending on the specific topological map

64. برای محاسبه
احتمال مشاهده رویداد i در موقعیت n در احتمال عدم مشاهده رویدادی در تمامی موقعیت های بین n و n’ ضرب میشود.

65. مثالی از محاسبه مقدار احتمال

66. مثالی از محاسبه مقدار احتمال
فرض کنید که روبات فقط دارای دو وضعیت با مقدار احتمال غیر صفر باشد:
p(1-2) = 1.0 ; p(2-3) = 0.2
اگر روبات به یقین به سمت چپ در حال حرکت باشد از حالت 2-3 به حالت 3 و سپس به 3-4 و 4 خواهیم رسید.
حال اگر روبات یک راهرو در سمت چپ خود و یک در باز در سمت راست خود مشاهده کرده باشد:
احتمال بودن در 3و 3-4 حذف خواهند شد زیرا احتمال تشخیص یک در باز وقتی که دیوار ی وجود دارد صفرا ست.
احتمال رسیدن به حالت 4 عبارت است
P(4)=A*B*C
=A حاصلضرب احتمال اولیه بودن در وضعیت 2-3
=Bاحتمال عدم مشاهده چیزی در وضعیت 3 و 3-4
=Cاحتمال مشاهده یک راهرو در سمت چپ و یک در در سمت راست در گره 4

67. مثالی از محاسبه مقدار احتمال
=A حاصلضرب احتمال اولیه بودن در وضعیت 2-3
=Bاحتمال عدم مشاهده چیزی در وضعیت 3 و 3-4
=Cاحتمال مشاهده یک راهرو در سمت چپ و یک در در سمت راست در گره 4
بنا به مقادیر احتمال اولیه داریم: A=0.2
B وقتی رخ میدهد که روبات در حالت 3 درب بسته و یا باز را در سمت چپ تشخیص نداده و بدرستی در سمت راست چیزی نیافته باشد. برای سادگی احتمال عدم یافتن چیزی در 3-4 یک فرض شده است.
B=0.6*0.4+(1-0.6)*0.05*.7
C وقتی رخ میدهد که روبات بدرستی راهرو را در سمت چپ تشخیص داده و به اشتباه به جای راهرو سمت راست یک درب تشخیص دهد .C=0.9*0.1
P(4)=0.003
توجه شود که مقادیر احتمال فوق likelihood هستد لذا مجموع آنها 1 نخواهد شد.

68. مثالی از محاسبه مقدار احتمال
به روش مشابهی میتوان نشان داد که اگر روبات از حالت 1-2 شروع نماید احتمال بودن در حالتهای 2-3و3و 3-4 صفر بوده و برای حالتهای 2 و 4 خواهیم داشت:
p(4) =4.3*10-6وp(2) = 0.3.
که برای حالت 4 این مقدار به مقدار قبلی اضافه خواهد شد.
از اینرو باور وضعیت جدید روبات به {2,4}تغییر خواهد نمود که وضعیت 2 دارای احتمال بیشتری خواهد بود.

69. نحوه برنامه ریزی برای حرکت
برای اینکه روبات بتواند حرکت خود در محیط را برنامه ریزی نماید روبات باید بتواند یکی ار موقعیت های موجود در نقشه را بعنوان موقعیت فعلی خود برگزیند.
معمولا این انتخاب موقعیتی خواهد بود که دارای بیشترین احتمال است. درحالتی که فاصله بین وضعیت با بیشترین احتمال و وضعیت با احتمال بعدی خیلی کم باشد تصمیم گیری دشوار شده و روبات مجبور خواهد بود تا مدتی در محیط به حرکت خود ادامه دهد تا به قطعیت در مورد موقعیت برسد.

70. مثالی از مکان یابی به روش مارکف با استفاده از یک نقشه شبکه ای
روبات این مثال دارای 24 سونار و یک فاصله یاب لیزری است و از یک نقشه grid based استفاده میکند.
نقشه متریک محیط بصورت یک شبکه 2 بعدی با اندازه سلول 4 تا 64 سانتیمترنشان داده میشود.
باور موقعیت بصورت 3 بعدی و با یک آرایه 15x15x15 برای مختصات x,y,q نشان داده میشود.

71. به روز رسانی باور وضعیت به روزرسانی action
اگر در لحظه t انکدر اندازه گیری o را داشته باشد احتمال بودن در موقعیت l برابر است با:
به روزرسانی perception
احتمال بودن در موقعیت l با اندازه گیری فاصله i برابر است با:

72. محاسبه مقدار احتمال
در این مثال نیز محاسبه p(i|l) کار سختی است بویژه اینکه تعداد حالات وضعیت بسیار زیاد است.
لذا این مقدار مستقیما از روی مدل عملکرد سنسور و موقعیت روبات و نقشه متریک ساخته میشود.
برای اینکار از فرضهای زیر استفاده میشود:
برای اشیا تشخیص داده شده توسط سنسور فاصله خطای موقعیت را میتوان با تابع توزیع احتمالی که میانگین آن در موقعیت صحیح قرار دارد نشان داد.
احتمال عدم اندازه گیری توسط سنسور فاصله صفر است
برای مواقعی که اندازه گیری سنسور فاصله از حد میگذرد تابع توزیع احتمال به پیک خود خواهد رسید.

74. The 1D case
Start
No knowledge at start, thus we have an uniform probability distribution.
Robot perceives first pillar
Seeing only one pillar, the probability being at pillar 1, 2 or 3 is equal.
Robot moves
Action model enables to estimate the new probability distribution based on the previous one and the motion.
Robot perceives second pillar
Base on all prior knowledge the probability being at pillar 2 becomes dominant

75. مکان یابی با استفاده از فیلتر کالمن
یک روبات متحرک ممکن است از سنسورهای متعدی استفاده نماید که هرکدام نشانه هائی را در مورد مکان روبات در بر خواهند داشت. از اینرو با استفاده از sensor fusionممکن است بتوان تخمین بهتری از موقعیت روبات بدست آورد.
فیلتر کالمن روش قدرتمندی است که امکان ترکیب داده های سنسورهای مختلف را فراهم میاورد.
این روش نسبت به روش مارکف ساده تر است. اما منجر به یک الگوریتم برگشتی میگردد .

76. شمای کلی فیلتر کالمن
System error source
فیلتر کالمن یک مکانیزم ریاضی است که با استفاده ازاطلاعاتی در مورد سیستم و وسیله اندازه گیری تخمینی از حالت سیستم بدست میدهد.
Control
System
System State
(Unknown)
Optimal estimate of system state
Observed measurement
Kalman Filter
Measurement Device
Measurement error source

77. مقدمه ای بر تئوری فیلتر کالمن
فیلتر کالمن امکان ترکیب چندین اندازه گیری را برای رسیدن به یک تخمین واحد از موقعیت سیستم فراهم میاورد.
در ابتدا مسئله را برای حالت استاتیک که در آن روبات بین دو اندازه گیری ثابت مانده است بررسی و سپس نتایج به حالت دینامیک تعمیم داده خواهد شد.
فرض های فیلتر کالمن
سیستم خطی فرض میشود
نویز سفید و گاوسی در نظر گرفته میشود

78. مقدمه ای بر تئوری فیلتر کالمن
فیلتر کالمن باور نسبت به موقعیت را بصورت یک توزیع گاوسی نشان میدهد
A 1-d Gaussian
distribution is given by:
An n-d Gaussian
distribution is given by:

79. تخمین استاتیک
روباتی را در نظر بگیرید که دارای دو سنسور لیزر و سونار برای اندازه گیری فاصله است. لیزر اطلاعات دورتر و دقیق تری را بوجود میاورد اما در مواردی نظیر وجود شیشه قادر به اندازه گیری درست فاصله نیست.
انتظار میرود که با ترکیب اطلاعات دو سنسور بتوان بهره اطلاعات را افزایش داد.
اگر در لحظه k یک اندازه گیری با لیزر و در لحظه k+1 اندازه گیری دیگری با سونار انجام شده باشد با فرض اینکه سنسورها دارای نویز unimodal گوسی با میانگین صفر باشند میتوان با فیلتر کالمن اندازه گیری دو سنسور را ترکیب نمود.

80. تخمین استاتیک
موقعیت روبات را میتوان از طریق هر یک از اندازه گیری های بصورت جداگانه تخمین زد:
q1 تخمین حاصل از سونار
q2 تخمین حاصل از لیزر
این اندازه گیری ها را میتوان با توزیع احتمال گوسی مدل نمود:

81. ترکیب داده ها
با فرض ثابت بودن روبات در بین اندازه گیری ها میتوان اطلاعات را بصورت زیر باهم ترکیب نمود.
Weighted least square
که در آن wi وزن اندازه گیری i میباشد. برای پیدا کردن خطا میتوان مشتق Sرا برابر صفر در نظر گرفت

82. ترکیب داده ها 
اگر مقدار وزن را بصورت زیر اختیار کنیم:
خواهیم داشت

83. ترکیب داده ها از اینرو میتوان نوشت:
این رابطه به شکل فیلتر کالمن بصورت زیر نوشته میشود:
که در آن
واریانس تخمین جدید برابر است با

84. حالت دینامیک
اگر روبات بین دو اندازه گیری در حال حرکت باشد میتوان حرکت آنرا با سرعت u و نویز w مدل نمود:
اگر از لحظه k که در آن واریانس موقعیت روبات و واریانس نویز حرکت مشخص است، شروع نمائیم، برای لحظه k’ خواهیم داشت:
از اینرو تخمین بهینه ای از موقعیت روبات قبل از اندازه گیری در لحظه t+1 خواهد بود

85. حالت دینامیک
معادلات کالمن بصورت زیر خواهند بود:

86. کاربرد فیلتر کالمن در روباتیک
Encoder
Position Prediction
Observation Prediction
Position estimate
Estimation
(Fusion)
Matched predictions and
Actual observation
Prediction observations
Map
Matching
Raw sensor data Or
extracted features
Actual Observation
Perception

87. مثالی از کاربرد کالمن فیلتر
روبات از لیزر برای اندازه گیری فاصله استفاده میکند.
محیط بانقشه ای شامل مجموعه ای از خطوط محدود نشان داده میشود.
باور وضعیت روبات توسط یک توزیع گاوسی نشان داده میشود.
فریم مختصات مورد استفاده برای سنسورها {S} با فریم روبات {R} یکسان است.
بردار ها در فریم مرجع {W} نشان داده میشوند.

88. تخمین موقعیت روبات
موقعیت روبات در لحظه k+1 با استفاده از موقعیت قبلی آن در لحظه k و مقدار حرکت آن در اثر اعمال کنترل u(t) بدست میاید.
واریانس این تخمین از رابطه زیر بدست میاید.
f: Odometry function

89. Robot Position Prediction: Example
تخمین موقعیت روبات در لحظه k+1 با استفاده از تخمین قبلی و در نظر گرفتن داده های انکدر ها بدست میاید.
برای روبات دیفرانسیلی داریم:
که ماتریس کوواریانس آن از روابط زیر بدست میاید.

90. Robot Position Prediction: Example
در لحظه k موقعیت روبات با p(k)=[x(k) y(k) q(k)]T وبهتربن تخمین موقعیت آن با p^(k|k) نشان داده میشود.
ورودی کنترلی u(k) روبات را به موقعیت p(k+1)میبرد.

91. مشاهده در این مرحله در لحظه k+1 اندازه گیری z(k+1) انجام میشود.
این مشاهده میتواند بر اثر اعمال فرایند استخراج ویژگی بر داده های خام سنسورها باشد که یک خط و یا یک درب را مشخص کرده باشد.
پارامترهای ویژگی استخراج شده معمولا در فریم مختصات محلی بیان میشود . برای تطبیق این ویژگی مشاهده شده باید آنرا از فریم مرجع به فریم سنسورها تبدیل نمود.
The parameters of the targets are usually observed in the sensor frame
}S}.
Therefore the observations have to be transformed to the world f rame {W} or
the measurement prediction have to be transformed to the sensor frame {S}.
This transformation is specified in the function hi (seen later).

92. Raw Data of laser scanner
Observation: Example
این روبات با استفاده از لیزردوار نقاطی را اندازه گیری کرده و از این داده ها خطوط موجود در محیط را استخراج میکند.
Raw Data of laser scanner
Extracted Lines

93. Observation: Example
پارامترهای هر خط در مختصات قطبی با aj,rj مشخص میشوند.
برای هر خط مقدار عدم قطعیت بدست آمده و ماتریس کوواریانس حاصله استخراج میشود.

94. تخمین اندازه گیری
در این مرحله با استفاده از نقشه محیطM(K) و موقعیتی که برای روبات تخمین زده شده است p^(k+1|k) ، تخمینی از ویژگی های مورد انتظارzt,i محاسبه میشود.
تابع hi برای تبدیل بین مختصات مرجع و سنسور)روبات ( بکار میرود.

95. Measurement Prediction: Example
با استفاده از نقشه محیط و موقعیتی که برای روبات تخمین زده شده است p^(k+1|k) ، تخمینی از ویژگی های مورد انتظارzt,i محاسبه میشود.
معمولا خطوط موجود در نقشه در فریم مرجع بیان میشوند که باید به فریم روبات منتقل شوند.

96. Measurement Prediction: Example
بر طبق شکل قبل این تبدیل مختصات بصورت زیر خواهد بود:
که ژاکوبین آن بصورت زیر خواهد بود:

97. Measurement Prediction: Example
تخمین اندازه گیری منجر به تخمین خطوطی خواهد شد که در فریم روبات بیان میشوند. این تخمین نادقیق خواهد بود زیرا تخمین موقعیت روبات نادقیق است.

98. تطبیق در این مرحله دارای دو دسته اطلاعات هستیم:
یکی ویژگی های مشاهده شده است
و دیگری مجموعه ای از ویژگی های تخمین زده شده است.
هر دو این داده ها در مختصات سنسور بیان شده اند.
هدف از مرحله تطبیق این است که مشخص نمائیم اندازه گیری روبات به کدامیک از ویژگی های تخمین زده شده در روی نقشه مربوط است.

99. تطبیق در این مرحله ارتباط بین ویژگی های مشاهده شده zj(k+1)
و ویژگی هدف )که در نقشه ذخیره شده است(، zt پبدا میشود.
برای اینکار فاصله بصورت زیر محاسبه میشود
مقدار کوواریانس آن بصورت زیر خواهد بود

100. Matching: Example

101. مرحله تخمین: اعمال فیلتر کالمن
در این مرحله بهترین تخمین برای موقعیت روبات با استفاده از تخمین موقعیت و همه مشاهدات در لحظه k+1 انجام میشود.
Kalman filter gain:
Update of robot’s position estimate:
The associate variance

102. مرحله تخمین: اعمال فیلتر کالمن
برای حالت یک بعدی خواهیم داشت:

103. Estimation: Applying the Kalman Filter
Kalman filter gain:
Update of robot’s position estimate:
The associate variance

104. مسائل ناشی از اعمال مدل خطی
اغلب مسائل مورد علاقه از جمله روباتهای متحرک غیر خطی هستند
در چنین مواردی باید سیستم غیر خطی را به نحوی بصورت خطی مدل کرد
میتوان با استفاده از معادلات انتشار حالت و مدلهای سنسورها را بصورت خطی درآورد
با خطی کردن روابط خطای حالت افزایش خواهد یافت.