機器人粒子濾波定位（蒙特卡羅定位）

機器人定位問題

General schematic for mobile robot localization

　　以下面的兩幅圖a、b為例，對移動機器人定位問題進行說明。假如機器人從一個已知的位置開始運動，利用里程計(編碼器)可以對其運動進行跟蹤，由於里程計的誤差，運動一段時間后，機器人對其位置的不確定度將變得很大。為了使位置的不確定性不至於無限增大，機器人必須參考其外部環境對自己進行定位。為了進行定位，機器人會使用外部傳感器（例如，超聲、激光、視覺傳感器等）對環境進行測量。如下圖a所示的預測階段：起始位置x₀已知，所以該位置上概率密度函數是δ函數（狄拉克函數）。當機器人開始移動，由於里程計的誤差，它的不確定性增加，並隨時間積累（In short, mobile robot effectors introduce uncertainty about future state. Therefore the simple act of moving tends to increase the uncertainty of a mobile robot）。下圖b所示為感知階段：機器人利用外部傳感器測量到右牆的距離為d，計算出當前位置為x^'_2，這與預測的位置x₂有沖突。融合兩個估計的概率密度，將位置校正，其不確定性得到減小（圖b中的實線）。

---

 The MCL Algorithm 

1. Randomly generate a bunch of particles

　　Particles can have position, heading, and/or whatever other state variable you need to estimate. Each has a weight (probability) indicating how likely it matches the actual state of the system. Initialize each with the same weight.

2. Predict next state of the particles

　　Move the particles based on how you predict the real system is behaving.

3. Update

　　Update the weighting of the particles based on the measurement. Particles that closely match the measurements are weighted higher than particles which don't match the measurements very well.

4. Resample

　　Discard highly improbable particle and replace them with copies of the more probable particles.

5. Compute Estimate

　　Optionally, compute weighted mean and covariance of the set of particles to get a state estimate.

　　粒子濾波定位又稱為蒙特卡羅定位（MCL, or Monte Carlo Localization, a localization algorithm based on particle filters）。粒子濾波的基本步驟為上面所述的5步，其本質是使用一組有限的加權隨機樣本（粒子）來近似表征任意狀態的后驗概率密度。粒子濾波的優勢在於對復雜問題的求解上，比如高度的非線性、非高斯動態系統的狀態遞推估計或概率推理問題。下面考慮一個2維平面內機器人定位的問題。機器人定位技術可分為相對定位和絕對定位兩類。相對定位是通過測量機器人相對於初始位置的距離和方向來確定機器人的當前位置，通常稱為航跡推算法，常用的傳感器包括里程計及慣性導航系統(陀螺儀、加速度計等)；絕對定位主要采用導航信標、主動或被動標識、地圖匹配或衛星導航技術進行定位。下圖中的機器人采用路標定位，路標一般定義為環境中被動的物體，當它們處在機器人傳感器測量范圍內時，提供了高的定位准確度。基於路標的導航中，一般路標都有固定的已知位置，機器人自身傳感器能對路標進行識別，通過與路標間的相對定位實現機器人在地圖中的絕對定位。如下圖所示，機器人通過視覺傳感器識別球場邊上用特定顏色標記的柱子(landmarks)來定位。

　　基本的MCL算法步驟如下：

　　即用M個粒子來描述狀態向量的后驗概率分布。算法第4行從系統轉移模型中取樣，將這些取樣的粒子作為先驗值。第5行使用測量模型修正粒子的權值。下面舉一個簡單的例子，來理解MCL算法：

　　如下圖(a)所示，機器人在水平方向沿一維直線移動。為了推測自身位置，初始時刻在其運動范圍內隨機生成均勻分布的粒子。粒子的X軸坐標表示推測的機器人位置，Y坐標表示處於這個位置的概率。機器人通過傳感器探測門時，按照算法第5行，MCL將修正每個粒子的權值。結果如圖(b)所示，可以看到與測量結果接近的粒子被賦予更大的權值。這里要注意，與圖(a)相比粒子的X坐標並未被改變，改變的只是權值。

　　圖(c)為重采樣后的粒子分布(line 8-11 in the algorithm MCL). This leads to a new particle set with uniform importance weights, but with an increased number of particles near the three likely places. 即重采樣后粒子權值歸一，但是在機器人可能出現的位置處，粒子數目開始變多。接下來再進行測量校正，如圖(d)所示。At this point, most of the cumulative probability mass is centered on the second door, which is also the most likely location. 

　　Further motion leads to another resampling step, and a step in which a new particle set is generated according to the motion model (Figure 8.11e). As should be obvious from this example, the particle sets approximate the correct posterior, as would be calculated by an exact Bayes filter.

　　下面我們來看一個2維平面上的例子，機器人在二維平面上移動，其轉向和速度作為控制輸入信號，機器人自身傳感器可以測量其與地圖中4個路標之間的距離。下面左圖是采用粒子濾波法估計機器人位置的第一次迭代，右圖為第10次迭代，圖中紅色的'X'表示小車真實位置，紅色圓圈表示粒子濾波計算出的位置。

　　下面的動態圖顯示了粒子濾波的整個過程：

　　從圖中可以看出第一次迭代后粒子仍然大量隨機的分散在地圖中，但是有一部分粒子已經開始聚集在小車的位置處。粒子濾波算出的結果與小車位置接近，這是因為離測量結果越接近的粒子賦予的權重越大，比如當小車運動到坐標(1,1)時，離(1,1)越近的點權值越大。小車位置的估計值通過粒子的加權平均計算出來，離小車真實值越近的粒子在估計中所占的比重越大。

　　步驟1：生成粒子

　　為了估計機器人真實位置，我們選取機器人位置x,y和其朝向θ作為狀態變量，因此每個粒子需要有三個特征。用矩陣particles存儲N個粒子信息，其為N行3列的矩陣。下面的代碼可以在一個區域上生成服從均勻分布的粒子或服從高斯分布的粒子：

from numpy.random import uniform,randn

def create_uniform_particles(x_range, y_range, hdg_range, N):
    particles = np.empty((N, 3))
    particles[:, 0] = uniform(x_range[0], x_range[1], size=N)
    particles[:, 1] = uniform(y_range[0], y_range[1], size=N)
    particles[:, 2] = uniform(hdg_range[0], hdg_range[1], size=N)
    particles[:, 2] %= 2 * np.pi
    return particles

def create_gaussian_particles(mean, std, N):
    particles = np.empty((N, 3))
    particles[:, 0] = mean[0] + (randn(N) * std[0])
    particles[:, 1] = mean[1] + (randn(N) * std[1])
    particles[:, 2] = mean[2] + (randn(N) * std[2])
    particles[:, 2] %= 2 * np.pi
    return particles

　　步驟2：利用系統模型預測狀態

　　每一個粒子都代表了一個機器人的可能位置，假設我們發送控制指令讓機器人移動0.1米,轉動0.7弧度，我么可以讓每個粒子移動相同的量。由於控制系統存在噪聲，因此需要添加合理的噪聲。 

def predict(particles, u, std, dt=1.):
    """ move according to control input u (heading change, velocity)
    with noise Q (std heading change, std velocity)`"""

    N = len(particles)
    # update heading
    particles[:, 2] += u[0] + (randn(N) * std[0])
    particles[:, 2] %= 2 * np.pi

    # move in the (noisy) commanded direction
    dist = (u[1] * dt) + (randn(N) * std[1])
    particles[:, 0] += np.cos(particles[:, 2]) * dist
    particles[:, 1] += np.sin(particles[:, 2]) * dist

　　步驟3：更新粒子權值

　　使用測量數據(機器人距每個路標的距離)來修正每個粒子的權值，保證與真實位置越接近的粒子獲得的權值越大。由於機器人真實位置是不可測的，可以看作一個隨機變量。根據貝葉斯公式可以稱機器人在位置x處的概率P(x)為先驗分布密度(prior)，或預測值，預測過程是利用系統模型(狀態方程)預測狀態的先驗概率密度，也就是通過已有的先驗知識對未來的狀態進行猜測。在位置x處獲得觀測量z的概率P(z|x)為似然函數(likelihood)。后驗概率為P(x|z)， 即根據觀測值z來推斷機器人的狀態x。更新過程利用最新的測量值對先驗概率密度進行修正，得到后驗概率密度，也就是對之前的猜測進行修正。

　　貝葉斯定理中的分母P(z)不依賴於x，因此，在貝葉斯定理中因子P(z)^-1常寫成歸一化因子η。

　　可以看出，在沒有測量數據可利用的情況下，只能根據以前的經驗對x做出判斷，即只是用先驗分布P(x)；但如果獲得了測量數據，則可以根據貝葉斯定理對P(x)進行修正，即將先驗分布與實際測量數據相結合得到后驗分布。后驗分布是測量到與未知參數有關的實驗數據之后所確定的分布，它綜合了先驗知識和測量到的樣本知識。因此基於后驗分布對未知參數做出的估計較先驗分布而言更有合理性，其估計的誤差也較小。這是一個將先驗知識與測量數據加以綜合的過程，也是貝葉斯理論具有優越性的原因所在。

---

 　　貝葉斯濾波遞推的基本步驟如下：

　　式子中概率p(x_t|u_t,x_t-1)是機器人的狀態轉移概率，它描述了機器人在控制量u_t的作用下從前一個狀態x_t-1轉移到下一狀態x_t的概率( It describes how much the x changes over one time step)。 由於真實環境的復雜性（比如控制存在誤差、噪聲或模型不夠准確），機器人根據理論模型推導下一時刻的狀態時，只能由概率分布p(x_t|u_t,x_t-1)來描述，而非准確無誤的量。參考Probabilistic Robotics中的一個例子：t-1時刻門處於關閉狀態，然后在控制量U_t的作用下機器人去推門，則t時刻門被推開的概率為0.8，沒有被推開的概率為0.2

p(X_t = open | U_t = push, X_t-1 = closed) = 0.8
p(X_t = closed | U_t = push, X_t-1 = closed) = 0.2

　　然而一般情況下，概率p(x_t|u_t,x_t-1)及p(z_t|x_t)的分布是未知的，僅對某些特定的動態系統可求得后驗概率密度的解析解。比如當p(x₀)，p(x_t|u_t,x_t-1)及p(z_t|x_t)都為高斯分布時，后驗概率密度仍為高斯分布，並可由此推導出標准卡爾曼濾波。

---

 　　這一步是非常關鍵的一步，即如何根據預測值和測量值修正粒子的權值。函數update的參數particles可以看做先驗概率分布p(x_t|u_t,x_t-1)的取樣值。假設傳感器噪聲符合高斯分布，那么在位置x_t處獲得觀測量z_t的概率p(z_t|x_t)可以由scipy.stats.norm(dist, R).pdf(z[i])來描述。由於多個路標特征之間的觀測相互獨立，因此符合概率乘法公式的條件，即下面代碼第5行中的累乘。

def update(particles, weights, z, R, landmarks):
    weights.fill(1.)
    for i, landmark in enumerate(landmarks):
        dist = np.linalg.norm(particles[:, 0:2] - landmark, axis=1)
        weights *= scipy.stats.norm(dist, R).pdf(z[i])

    weights += 1.e-300      # avoid round-off to zero
    weights /= sum(weights) # normalize

　　步驟4：重采樣

　　在計算過程中，經過數次迭代，只有少數粒子的權值較大，其余粒子的權值可以忽略不計，粒子權值的方差隨着時間增大，狀態空間中的有效粒子數目減少，這一問題稱為權值退化問題。隨着無效粒子數目的增加，大量計算浪費在幾乎不起作用的粒子上，使得估計性能下降。通常采用有效粒子數N_eff衡量粒子權值的退化程度。N_eff的近似計算公式為：

　　有效粒子數越小，表明權值退化越嚴重。當N_eff的值小於某一閾值時，應當采取重采樣措施，根據粒子權值對離散粒子進行重采樣。重采樣方法舍棄權值較小的粒子，代之以權值較大的粒子，有點類似於遺傳算法中的“適者生存”原理。重采樣的方法包括多項式重采樣(Multinomial resampling)、殘差重采樣(Residual resampling)、分層重采樣(Stratified resampling)和系統重采樣(Systematic resampling)等。重采樣帶來的新問題是，權值越大的粒子子代越多，相反則子代越少甚至無子代。這樣重采樣后的粒子群多樣性減弱，從而不足以用來近似表征后驗密度。克服這一問題的方法有多種，最簡單的就是直接增加足夠多的粒子，但這常會導致運算量的急劇膨脹。其它方法可以去查看有關文獻，這里暫不做介紹。

　　重采樣的基本步驟如下：

　　簡單的多項式重采樣(Multinomial resampling)代碼如下：

def simple_resample(particles, weights):
    N = len(particles)
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1. # avoid round-off error
    indexes = np.searchsorted(cumulative_sum, random(N))

    # resample according to indexes
    particles[:] = particles[indexes]
    weights[:] = weights[indexes]
    weights /= np.sum(weights) # normalize

　　步驟5：計算狀態變量估計值

　　系統狀態變量估計值可以通過粒子群的加權平均值計算出來。

def estimate(particles, weights):
    """returns mean and variance of the weighted particles"""

    pos = particles[:, 0:2]
    mean = np.average(pos, weights=weights, axis=0)
    var  = np.average((pos - mean)**2, weights=weights, axis=0)
    return mean, var

　　下面是完整的程序：

import numpy as np
import scipy.stats
from numpy.random import uniform,randn,random
import matplotlib.pyplot as plt


def create_uniform_particles(x_range, y_range, hdg_range, N):
    particles = np.empty((N, 3))
    particles[:, 0] = uniform(x_range[0], x_range[1], size=N)
    particles[:, 1] = uniform(y_range[0], y_range[1], size=N)
    particles[:, 2] = uniform(hdg_range[0], hdg_range[1], size=N)
    particles[:, 2] %= 2 * np.pi
    return particles

    
def predict(particles, u, std, dt=1.):
    """ move according to control input u (heading change, velocity)
    with noise Q (std heading change, std velocity)"""

    N = len(particles)
    # update heading
    particles[:, 2] += u[0] + (randn(N) * std[0])
    particles[:, 2] %= 2 * np.pi

    # move in the (noisy) commanded direction
    dist = (u[1] * dt) + (randn(N) * std[1])
    particles[:, 0] += np.cos(particles[:, 2]) * dist
    particles[:, 1] += np.sin(particles[:, 2]) * dist    
    
    
def update(particles, weights, z, R, landmarks):
    weights.fill(1.)
    for i, landmark in enumerate(landmarks):
        distance = np.linalg.norm(particles[:, 0:2] - landmark, axis=1)
        weights *= scipy.stats.norm(distance, R).pdf(z[i])

    weights += 1.e-300      # avoid round-off to zero
    weights /= sum(weights) # normalize    
    
    
def estimate(particles, weights):
    """returns mean and variance of the weighted particles"""

    pos = particles[:, 0:2]
    mean = np.average(pos, weights=weights, axis=0)
    var  = np.average((pos - mean)**2, weights=weights, axis=0)
    return mean, var    
    
    
def neff(weights):
    return 1. / np.sum(np.square(weights))    
    
    
def simple_resample(particles, weights):
    N = len(particles)
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1. # avoid round-off error
    indexes = np.searchsorted(cumulative_sum, random(N))

    # resample according to indexes
    particles[:] = particles[indexes]
    weights[:] = weights[indexes]
    weights /= np.sum(weights) # normalize    

    
def run_pf(N, iters=18, sensor_std_err=0.1, xlim=(0, 20), ylim=(0, 20)):    
    landmarks = np.array([[-1, 2], [5, 10], [12,14], [18,21]]) 
    NL = len(landmarks)
    
    # create particles and weights
    particles = create_uniform_particles((0,20), (0,20), (0, 2*np.pi), N)
    weights = np.zeros(N)
    
    xs = []   # estimated values
    robot_pos = np.array([0., 0.])
    
    for x in range(iters):
        robot_pos += (1, 1) 
        
        # distance from robot to each landmark
        zs = np.linalg.norm(landmarks - robot_pos, axis=1) + (randn(NL) * sensor_std_err)
        
        # move particles forward to (x+1, x+1)
        predict(particles, u=(0.00, 1.414), std=(.2, .05))
        
        # incorporate measurements
        update(particles, weights, z=zs, R=sensor_std_err, landmarks=landmarks)
        
        # resample if too few effective particles
        if neff(weights) < N/2:
            simple_resample(particles, weights)
        
        # Computing the State Estimate
        mu, var = estimate(particles, weights)
        xs.append(mu)
    
    xs = np.array(xs)
    plt.plot(np.arange(iters+1),'k+')
    plt.plot(xs[:, 0], xs[:, 1],'r.')
    plt.scatter(landmarks[:,0],landmarks[:,1],alpha=0.4,marker='o',c=randn(4),s=100) # plot landmarks
    plt.legend( ['Actual','PF'], loc=4, numpoints=1)
    plt.xlim([-2,20])
    plt.ylim([0,22])
    print 'estimated position and variance:\n\t', mu, var
    plt.show()
    
    
    
if __name__ == '__main__':    
    run_pf(N=5000)

　　圖中4個顏色各異的大圓點是路標，紅色小圓點是粒子濾波估計出的機器人位置：

　　

 

 

參考：

Probabilistic Robotics chapter 8 p200

零基礎概率論入門：貝葉斯推斷

https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python

http://www.cs.utexas.edu/~teammco/misc/particle_filter/

Particle Filter Tutorial 粒子濾波：從推導到應用（一）

Particle Filter Tutorial 粒子濾波：從推導到應用（三）

Particle Filter Tutorial 粒子濾波：從推導到應用（四）