Hello 大家好，今天給大家?guī)硪环蓐P(guān)于定位的概述入門性質(zhì)的文章辣垒，看完這篇文章，你將了解Location的意義印蔬，以及一個(gè)最基本1D直方圖濾波的Demo勋桶，如果你只是需要源代碼，請(qǐng)直接往下翻侥猬！
一例驹、Location的意義
在機(jī)器人導(dǎo)航任務(wù)中，location 可以告訴機(jī)器人目前位置退唠，以方便閉環(huán)控制或者軌跡規(guī)劃鹃锈。一般情況下，Location 可以通過
GPS瞧预，WIFI 等方式完成屎债。GPS的定位精度在3.5米左右仅政，WIFI則大于10米。對(duì)于機(jī)器人盆驹、無人汽車而言圆丹，這樣的精度顯然是不可接受的。激光雷達(dá)在10m的距離可以達(dá)到cm的精度召娜，雙目視覺在4m可以達(dá)到10cm的精度运褪，與GPS相比有一定優(yōu)勢(shì)，此外玖瘸，這種非在線的定位方式可以在室內(nèi)使用秸讹。
Location 最大的難度來自于測(cè)量誤差。里程計(jì)的計(jì)數(shù)誤差雅倒，測(cè)量與里程計(jì)的不統(tǒng)一等璃诀。所以我們需要一種算法，能夠較高精度的確定機(jī)器人的位姿蔑匣。
如上圖所示劣欢，那些環(huán)境的網(wǎng)格是一個(gè)個(gè)離散的單元，將連續(xù)的環(huán)境離散化裁良，面對(duì)的是一個(gè)離散的概率分布凿将，這樣子就意味著機(jī)器人可能處于的位置總數(shù)是有限的。下面我們將實(shí)例講解一個(gè)最基本的基于一維空間的網(wǎng)格Location的直方圖濾波算法价脾，以初步定性的理解機(jī)器人Location算法牧抵。
下面的列子Udacity上的課程 https://cn.udacity.com/course/artificial-intelligence-for-robotics–cs373/
二、直方圖濾波
定義一個(gè)一維的網(wǎng)格空間
首先我們定義一個(gè)一維的網(wǎng)格空間侨把，如下圖所示, 機(jī)器人就在這里糾結(jié)自己處于哪一個(gè)格子;
初始化概率分布
接著我們需要初始化相應(yīng)的概率分布：
初始概率
p=[0.2, 0.2, 0.2, 0.2, 0.2]
world=['green', 'red', 'red', 'green', 'green']
我們首先初始化沒一個(gè)網(wǎng)格的概率都占 0.2的均勻分布犀变，定義每一個(gè)網(wǎng)格的顏色，這樣子這個(gè)一維的網(wǎng)格地圖就初始化完畢了秋柄。
接著我們需要加入傳感器的信息获枝，以及機(jī)器人運(yùn)動(dòng)概率信息。
加入傳感器與運(yùn)動(dòng)
measurements = ['red', 'red']
motions = [1,1]
pHit = 0.6
pMiss = 0.2
pExact = 0.8
pOvershoot = 0.1
pUndershoot = 0.1
在機(jī)器人運(yùn)動(dòng)過程中骇笔，我們首先定義機(jī)器人的運(yùn)動(dòng)模式 motions 分別是 -1 和 1,即移動(dòng)方向左（-）右（+）省店，假定格子是循環(huán)的(首尾聯(lián)通);
接著我們定義pHit和pMiss，他們分別代表的是實(shí)際值與測(cè)量值相同的概率和不同的概率笨触，這個(gè)怎么理解喃懦傍，我們來看一下下面這張圖：
如圖所示旭旭，機(jī)器人測(cè)量到的信息是red葱跋， 真實(shí)的顏色分別是 ['green', 'red', 'red', 'green', 'green']源梭，那么機(jī)器人處于各個(gè)位置的概率就應(yīng)該這樣計(jì)算，首先如果處于第一個(gè)網(wǎng)格稍味，那么表示機(jī)器人檢測(cè)錯(cuò)了废麻，則處于一個(gè)網(wǎng)格的值應(yīng)該為：0.2 * 0.2 = 0.04，那么同理可得烛愧，若處于第二個(gè)網(wǎng)格掂碱，就表明檢測(cè)對(duì)了怜姿，則相應(yīng)的網(wǎng)格值為：0.6 * 0.2 = 0.12。
最終得到下面的數(shù)值分布：
['0.04', '0.12', '0.12', '0.04', '0.04', '0.04'];
注意,這里還不是概率分布疼燥，因?yàn)楹筒坏扔?,我們需要進(jìn)行歸一化處理沧卢，需要知道具體的代碼實(shí)現(xiàn)請(qǐng)繼續(xù)往下看。
然后但狭，我們定義了機(jī)器人自己運(yùn)動(dòng)正確到達(dá)每個(gè)位置的概率, 即使我們已經(jīng)可以預(yù)測(cè)他到達(dá)的位置撬即，但是實(shí)際運(yùn)動(dòng)總是存在誤差，可能是輪子打滑剥槐，遇到障礙物，中途沒電才沧，等等，因此我們有不足/超調(diào)的三種情況：
- pExact = 0.8
- pOvershoot = 0.1
- pUndershoot = 0.1
就這樣我們定義了所有的概率分布挨摸，接著讓我們實(shí)現(xiàn)具體的算法吧岁歉。
算法主體包含兩個(gè)部分，測(cè)量值的更新和運(yùn)動(dòng)更新锅移，最終得到各個(gè)網(wǎng)格的概率分布。
測(cè)量值更新
# update measurement
def sense(p, Z):
q=[]
for i in range(len(p)):
hit = (Z == world[i])
q.append(p[i] * (hit * pHit + (1-hit) * pMiss))
s = sum(q)
# Normalized Sense
for i in range(len(q)):
q[i] = q[i] / s
return q
我們首先需要獲取所有的測(cè)量值： for i in range(len(p)):置逻，之后分別與實(shí)際的world[i]比較备绽，之后乘上相應(yīng)的pHit / pMiss鬓催。
之后通過sum函數(shù)獲取總和用于皈依化處理恨锚，得到測(cè)量的概率分布。
運(yùn)動(dòng)更新
def move(p, U):
q = []
for i in range(len(p)):
s = pExact * p[(i-U) % len(p)]
s = s + pOvershoot * p[(i-U-1) % len(p)]
s = s + pUndershoot * p[(i-U+1) % len(p)]
q.append(s)
return q
我們通過len(p)獲取運(yùn)動(dòng)的次數(shù)猴伶，之后分別計(jì)算正確/超調(diào)/不足的概率，并且求和以得到運(yùn)動(dòng)之后的概率分布筝尾。
可視化：
然后我們需要可視化我們的得到的分布：
def display_map(grid, bar_width=1):
if(len(grid) > 0):
x_labels = range(len(grid))
plt.bar(x_labels, height=grid, width=bar_width, color='b')
plt.xlabel('Grid Cell')
plt.ylabel('Probability')
plt.ylim(0, 1) # range of 0-1 for probability values
plt.title('Probability of the robot being at each cell in the grid')
plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 1))
plt.show()
else:
print('Grid is empty')
通過python的matplotlib庫(kù)可以輕松的實(shí)現(xiàn)可視化办桨，我們可以通過定義如下的main函數(shù)，整合上述的工作：
def main():
global p
for k in range(len(measurements)):
p = sense(p, measurements[k])
p = move(p, motions[k])
print (p)
display_map(p)
if __name__ == '__main__':
main()
我們甚至可以開啟matplotlib實(shí)時(shí)顯示功能贸街，觀察如果我們運(yùn)動(dòng)1000次會(huì)有什么結(jié)果：
def main():
global p
plt.ion()
for k in range(len(measurements)):
p = sense(p, measurements[k])
for i in range(1000):
p = move(p, motions[k])
plt.pause(0.05)
display_map(p, 0.9)
print (p)
三狸相、總結(jié)一下
通過今天的這篇文章，我們介紹了一個(gè)最基本用于機(jī)器人定位的算法模型脓鹃，并且使用結(jié)合了Python與概率論的只是具體實(shí)現(xiàn)了它，別小看這個(gè)算法娇跟，他可是自動(dòng)駕駛汽車定位的核心思想！
