Localization (using Histogram Filters)

? ? ? ?定位指的是在傳感器和移動之間來回的迭代，使得能夠保持跟蹤目標對象的位置、方向和速度。

? ? ?? 這篇將寫一個程序來實施定位，與GPS相比，這個的程序將極大的降低誤差范圍。

? ? ? ?假設一個汽車或者機器人所處在一個一維世界，它在沒有得到任何提示在哪一位置。通過一個函數對這個問題建模，縱軸表概率，橫軸表這個一維世界里所有位置，利用一個當值函數給這個一維世界每一個地方分配相同權重。

? ? ??

? ? ? ?為了定位必須引入其他特征，假設有三個看起來相似的門，可以從非門區域 區分一扇門，（brlief? = 信度），機器人感受到了它在一扇門的旁邊，它分配這些地點更大的概率。門的度量改變了信度函數，得到新函數像這樣，三個臨近門的位置信度遞加，其他所有地方信度遞減，posterior bilief 表示它是在機器人進行感測測量后定義的。

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

?

?

? ? ? ? 如果機器人移動了，那么凸起的信度也會隨之移動（所在位置的概率），而且凸起會因為機器人只是粗略地知道移動了多遠而變得扁平化，這個過程叫卷積，卷積就是兩個函數或措施的重疊，具體的說是一個函數劃過另一個函數的重疊占比，介于0~1之間（CONVOLUTION 卷積）

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ??

? ? ? ?假設汽車或機器人發現它的右邊再一次靠近一扇門，此時在先于第二次測量的信度上乘以一個函數，函數如下，它在每一扇門下都有一個很小的凸起，但卻有一個非常大的凸起。在先驗上這是唯一一個真的很符合這個門的位置，所以其他的門有一個很低的信度，Posterior? belief 先驗信度

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

? ? ? 重要的是仍然不能確定我們的位置，但是之后，會比一次或零次測量之后的測量更確定。 你認為在做了越來越多的測量后會發生什么？

? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

? ? ? 這種定位稱為Monte Carlo 定位，也稱為直方圖濾鏡。

---

?

?

用Python程序化

sense( )

---

?

? ? ? ?新建一個列表，在這個一維世界中，假設有5個不同的單元或者說地方，用X1~X5標識，機器人落在每個單元的概率相同，概率的和加起來是1，P(Xi)表示落在其中任何一個單元格的概率。

1 #初始化向量 2 P = [0.2,0.2,0.2,0.2,0.2] 3 #創造一個長度為n的向量，可以通過變化n的值得到n個元素的向量 4 P =[ ] 5 n = ? 6 7 for i in range(n) 8 p.append(1./n) 9 print p

?

? ? ? ?有著5個單元格的一維世界的度量方式，假設有兩個被染成紅色，其他兩個是綠色，分配給每一個單元的概率是0.2，現在機器人允許被感知，它先看到胡是紅色，23的概率會上升，145的概率會下降，用乘法將這個度量合并到信度里。

? ? ? 可是這并不是一個有效的概率分布，因為概率加起來總要等于1。這些概率值的求和是0.36，為了變成有效的概率分布，把每個數除以0.36

? ? ? ?

? ? ? ? ?通過引入一個變量使代碼更加優雅，這個變量為每個小格指定紅色或者綠色。

? ? ? ?此外，先前說道機器人感覺到它在一個紅色的區間里，所以這里定義測量到的值為紅色。

? ? ? ?然后，寫一個叫sense的函數用在后面的定位器，它能對探測數據進行更新，輸入初始分布p、探測值Z以及其他所有的全局變量，輸出一個標準化分布q，q反應了輸入概率的非標準化乘積

P = [0.2,0.2,0.2,0.2,0.2] world = ['green','red','red','green','green'] #定義測量值為紅色
Z = 'red'
pHit = 0.6 pMiss = 0. 2def sense()q = [ ]for i in range(len(p)):#用了一個二元標記，hit =0或1hit = (Z == world[i]) #由于hit取0或1，所以每次只有一個算式成立
q.append[p[i]*(hit*pHit+(1-hit)*pMiss)] #以下代碼使輸出標準化（和等于1）
s = sum(q) for i in range(len(p))：q[i] = q[i] / sreturn q
print sense(p,z)

?

修改代碼，通過用包含多個測量值的測量值向量替代Z

假設機器人感覺到紅色，之后是綠色，修改代碼使得更新兩次概率，并在兩次測量結合后給出后驗分布，以便可以處理任何測量順序，無論長度如何。

p?=?[0.2,?0.2,?0.2,?0.2,?0.2] world?=?['green',?'red',?'red',?'green',?'green']? #替代Z measurements?=?['red',?'green'] pHit?=?0.6 pMiss?=?0.2 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)for?i?in?range?(len(p)):q[i]?=?q[i]/sreturn?q #抓取測試值向量中元素用來更新自己的概率
for?k?in?range(len(measurements)):?p?=?sense(p,?measurements?[k])print?p?

這個sense()就是定位中的關鍵函數——測量更新函數，對每個測量值調用一次這個函數并且更新概率分布

?

?move( )

---

假設世界是循環的，空間的概率分布如下，并且知道機器人向右移動，若機器人向右移動一個位置，運動后的后驗概率分布是多少？

?

定義move函數，輸入初始分布p和運動量U，其中U是向右或向左移動的網格的數量，返回新分布q，如果U=0，q=p

將網格2的概率從0變為1，使得向右移動一格后，以觀察到運動的效果

def move(p,U)q = []for i in range(len(p))q.append(p[(i - U) % len(p)]) #a very skilled coderenturn q
for?k?in?range(len(measurements)):
p?=?sense(p,?measurements?[k])
print?move?(p,?1）

?

機器人移動不是準確的，定位困難

需要建立更精確的機器人運動模型，假設機器人移動到目標地的概率是0.8，到目標地之前概率是0.1，超過目標概率是0.1。

已有初始分布，給出移動后的分布

?

?如預期，概率分布已經轉移和擴展。 移動導致信息丟失。

?

又一次，假設網格2和4的值為0.5，填充后驗分布（也考慮了未達到和超過兩種情況）。

再一次，給定均勻分布，填寫移動后分布。

明確計算到達每格可能方式太復雜，換種思路，如果從統一的先前分配（最大混淆狀態）開始，之后移動，必定最終得到均勻分布

?

p?=?[0,?1,?0,?0,?0) world?=?['green',?'red',?'red',?'green',?'green'] measurements?=?['red',?'green'] pHit?=?0.6 pMiss?=?0.2 #Add?exact?probability pExact?=?0.8 #Add?overshoot/undershoot probability pOvershoot?=?0.1 pUndershoot?=?0.1def?move(p,?U):#Introduce?auxiliary?variable?sq=?[]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

該函數適應了預期的低估或超調的可能性，通過遍歷p中的每個網格并且在q中的三個單元上（目標網格，距離U以及過沖和下沖網格）適當地分配其概率來移動目的地。

?

從初始分布p?=?[0,?1,?0,?0,?0]開始,移動兩次:

p?=?move(p,?1) p?=?move(p,?1) print?p >>>[0.01,?0.01,?0.16,?0.66,?0.16]

結果是一個向量，其中0.66是最大值，而不是0.8。 這是預期的：兩次移動已經平坦化，擴大了分布。

而后編寫移動一千步后得到的概率分布：

for?k?in?range(1000):p?=?move(p,?1)print?p#最終分布是如預期的每個網格0.2

?

機器人感知和移動

---

?

定位，特別是這里用到的Monte Carlo定位方法，只不過是傳感器和移動的迭代，有一個初始的感知拋給循環。每次移動都會丟失信息（因為機器人的移動是不準確的），每次感知會獲取信息，事實證明機器人移動之后概率會變得平展、延伸，感知之后會變得更加集中。 熵是衡量分布信息的度量。

????????????????????????????????????????? ? ??? ? ? ? ? ? ? ? ??

機器人從均勻的的先前分部開始，再向右移動兩次，首先感覺到是紅色，然后感覺到是綠色， 計算后驗分布

motions?=?[1,?1] p?=?[0.2,?0.2,?0.2,?0.2,?0.2] for?k?in?range(len(measurements)):p?=?sense(p,?measurements[k])p?=?move(p,?motions[k])print?p

查看結果概率分布，機器人最可能是從網格3出發

如果機器人感知到兩次紅色

1 p=[0.2, 0.2, 0.2, 0.2, 0.2] 2 world=['green', 'red', 'red', 'green', 'green'] 3 measurements = ['red', 'green'] 4 motions = [1,1] 5 pHit = 0.6 6 pMiss = 0.2 7 pExact = 0.8 8 pOvershoot = 0.1 9 pUndershoot = 0.1 10 11 def sense(p, Z): 12 q=[] 13 for i in range(len(p)): 14 hit = (Z == world[i]) 15 q.append(p[i] * (hit * pHit + (1-hit) * pMiss)) 16 s = sum(q) 17 for i in range(len(q)): 18 q[i] = q[i] / s 19 return q 20 21 def move(p, U): 22 q = [] 23 for i in range(len(p)): 24 s = pExact * p[(i-U) % len(p)] 25 s = s + pOvershoot * p[(i-U-1) % len(p)] 26 s = s + pUndershoot * p[(i-U+1) % len(p)] 27 q.append(s) 28 return q 29 30 for k in range(len(measurements)): 31 p = sense(p, measurements[k]) 32 p = move(p, motions[k]) 33 34 print p

結果表明機器人在網格4的概率大，也就是從網格2出發

?

這個代碼就是Google自動駕駛定位代碼的本質，也是精髓，特別重要的是，汽車準確知道它在哪里，對于它行駛的道路地圖而言，雖然道路不能被涂成綠色或紅色，但是道路有著路標代替紅色綠色的格子，不僅是每一步一次的觀察，而是整個場地的觀察，整個照相機圖片的觀察，可以對照片做同樣的事，只要能夠將照片嵌入到模型當中，用在度量當中，也不會比這個代碼難上多少代碼，而Google花費這么多時間構建自動駕駛產品因為現實情況會很復雜，道路會正在鋪設和重修，這些問題要處理。

?

定位總結

---

定位涉及機器人不斷更新其對所有可能位置的感知。 在數學上說，可以說機器人不斷更新其在樣本空間上的概率分布。 對于一輛真正的自駕車，這意味著道路上的所有位置都有一個概率。

如果AI正常工作，這種概率分布應該有兩個特征：

應該有一個尖峰。 這表明汽車對其當前位置有非常明確的感知。

峰值應該是正確的。

Monte Carlo定位程序可以寫成一系列步驟：

從當前位置的感知開始

通過感知測量結果乘以這個分布

歸一化所得到的分布

通過執行卷積移動。 這聽起來比真的更復雜：這個步驟實則涉及乘法和加法，以獲得每個新的概率位置。

?兩個數學方法

---

?

貝葉斯法則

在概率上，經常想在更新概率分布后測量。 有時這不容易直接做。

貝葉斯法則是進行測量后更新概率分布的工具。x是網格，Z是感知度量，貝葉斯法則方程式如下：

左邊應該被讀成“觀察Z后的X的概率”，P(x)是先驗分布，然后與P(Z|x)測量概率相乘，對于每個格子，可以放上下標i，P(Z)是每個格子如此操作后的和

一個用貝葉斯做癌癥測試例子

?

全概率定理

上面是測量，再看移動，在這里要關注一個網格“xi”，并且詢問機器人運動后在xi中的概率是什么。 現在，使用時間索引來表示運動前后：

?

P(Xi | Xj)是可從j到i的所有路徑概率，利用這個定理在移動前就能算出概率分布

舉個例子，拋一枚硬幣，反面什么也不做。正面再拋一次，求第二次拋出正面概率

?

?

編寫一個程序：

1 # 定位函數用到了以下參數: 2 # colors: 3 # 2D 列表, 'R' (紅色網格l) or 'G' (綠色網格) 4 # measurements: 5 # 機器人得到的測量值列表 ，'R' or 'G' 6 # motions: 7 # 機器人采取的動作列表，[dy，dx]，dx表示x方向的變化（正向右移動），dy表 # 示y方向的變化（正向下移動） 8 # 9 # sensor_right: 10 # 測量是正確的概率，浮動在0和1之間，不正確的概率是1-sensor_right 11 # p_move: 12 # 執行移動命令的概率，浮動在0和1之間，運動命令失敗的概率（仍保持原地）是 13 # 1-p_move; 機器人在本編程中不會超過目的地； 14 # 15 # 輸出：函數返回與顏色列表尺寸相同的2D概率分布列表，最初是均勻的概率分布 16 # 17 # 每一步先行動，再測量,且任何行列是循環的 18 19 def localize(colors,measurements,motions,sensor_right,p_move): 20 sensor_wrong = 1.0 - sensor_right 21 p_stay = 1.0 - p_move 22 23 if len(measurements) != len(motions) 24 raise ValueError,"error in size of measurement/motion vector" 25 26 pinit = 1.0 / float(len(colors)) / float(len(colors[0])) 27 p = [[pinit for row in range(len(colors[0]))] for col in range(len(colors))] 28 29 for k in range(len(measurements)): 30 p = move(p,measurements[k]) 31 p = sense(p,colors,measurements[k]) 32 33 show(p) 34 35 def sense(p,colors,measurement): 36 aux = [[0.0 for row in range(len(0)) for col in range (len(p))]] 37 s = 0.0 38 for i in range(len(p)): 39 for j in range(len(p[i])): 40 hit = (measurement == colors[i][j]) 41 aux = p[i][j]*(hit*sensor_right + (1-hit)*sensor_wrong) 42 s+=aux[i][j] 43 for i in range(len(aux)): 44 for j in range(len(p[i])): 45 aux[i][j] /= s 46 return aux 47 48 def move(p,motion): 49 aux = [[0.0 for row in range(len(0)) for col in range (len(p))]] 50 for i in range(len(p)): 51 for j in range(len(p[i])): 52 aux = (p_move*p[i-motion[0] % len(p)][j - motion[1] % len(p[i])] + (p_stay*p[i][j])) 53 return aux 54 55 def show(p): 56 rows = ['[' + ','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p] 57 print '[' + ',\n '.join(rows) + ']' 58 def show(p) 59 for i in range(len(p)) 60 print p[i] 61 62 63 64 ############################################################# 65 # For the following test case, your output should be 66 # [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465], 67 # [0.00715, 0.01017, 0.08696, 0.07988, 0.00935], 68 # [0.00739, 0.00894, 0.11272, 0.35350, 0.04065], 69 # [0.00910, 0.00715, 0.01434, 0.04313, 0.03642]] 70 # (within a tolerance of +/- 0.001 for each entry) 71 72 colors = [['R','G','G','R','R'], 73 ['R','R','G','R','R'], 74 ['R','R','G','G','R'], 75 ['R','R','R','R','R']] 76 measurements = ['G','G','G','G','G'] 77 motions = [[0,0],[0,1],[1,0],[1,0],[0,1]] 78 p = localize(colors,measurements,motions,sensor_right = 0.7, p_move = 0.8)

?

?實際設計工作中怎樣給這些概率賦值，例如pHit、pMiss等，從試驗中獲得還是傳感器參數決定？

? ? ? ?《概率機器人》--很大程度上由傳感器決定，谷歌大量使用激光雷達作為傳感器，但如果選擇攝像機就需要用與測距儀不同的模型，使用激光雷達時測的是距離而非pHit、pMiss，它給出的是在特定位置測得特定距離的概率。

是否要求機器人內置地圖？

? ? ? ?yes，實時定位與地圖創建SLAM

實際中的定位器是用來測什么的，顯然不是一扇門？

? ? ? ?汽車會內置地圖，激光攝像頭掃描周圍場景，其中包含與內置地圖相同的特征，要解決的概率匹配問題是將生成的附近地圖疊加到全局地圖上，檢查能獲得最佳匹配的位置。

全工況？

? ? ? ?雨，調高亮度參數，雪，未解決

轉載于:https://www.cnblogs.com/Real-Ying/p/7570642.html

總結

以上是生活随笔為你收集整理的Localization的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。