RoboCup中的機器人自主定位——從理論到實踐（二）：無跡卡爾曼濾波詳解

上一篇RoboCup中的機器人自主定位（一）我們詳細的介紹了RoboCup的背景，在最後給出了幾種常用的解決自主定位的算法。
這一篇主要講解方案的選擇，並提供一些必要的數學論證。整個寫作思路基本按照我做項目時的流程展開。

1.自定位的本質

定位本質上是要知道機器人的位置，而在2D平面座標系中座標就是用$(x,y)$來表示的。如果機器人身上裝有GPS或者我們的北斗導航系統，那這個$(x,y)$就可以藉助傳感器直接獲得，然而事實是室內的機器人並沒有這套定位系統，即使有定位系統由於房屋遮擋也無法精確定位，因此位置信息不能直接獲得。

既然直接的不行，我們就來“曲線救國”。機器人不是有“眼睛”嘛，那就可以通過觀測外部場景來獲取信息；機器人還知道自身的運動方向和加速度，那就爲下一個位置點的出現提供了預測的可能。這一思想的出現是至關重要的，因爲它將概率的思想引入了機器人學，通過概率分佈而不是某個確定的值來表示機器人的位姿。因此，這也就很好理解爲何那麼多的定位方法都是基於概率學基礎的了。

2.貝葉斯濾波

接下來的內容就需要一些數學知識了，不過請放心，這些內容只涉及基礎的概率論知識，而且我會用通俗易懂的方式來講解。說回機器人的位姿，這裏插一句，“位姿”就是位置和姿態的縮寫，表示的除了橫縱座標還有一個方向角，因此之後我們對機器人定位的描述寫成數學符號就是求$(x,y,\theta)$。好，剛纔說這三個狀態量無法直接獲得，需要通過機器人本身的信息和觀測數據進行推測，而既然是推測那就有可能準有可能不準，爲了表示這個推測量我們引入概率論中的置信度(belief)：$bel(x_t)=p(x_t|z_{1:t},u_{1:t})$
其中$x_t$表示$t$時刻的三個狀態量，寫成向量的形式，$z_{1:t}$表示1到$t$時刻所有的觀測量，$u_{1:t}$表示1到$t$時刻所有的控制量即輸入信息。很顯然，這就是一個以內部信息和外部觀測爲條件的後驗分佈。
那接下來的問題就是這個置信度怎麼求呢？這就需要用到大名鼎鼎的貝葉斯公式了（這也是爲什麼這個方法被稱之爲貝葉斯濾波了）
$P(A|B,C)=\frac{P(A,B,C)}{P(B,C)}$由該公式，再結合馬爾科夫假設、全概率公式等計算，就能得出貝葉斯濾波的這套流程，共分兩步走，分別被稱爲“預測”和“更新”。
在這裏進行一下通俗的解釋就是，系統根據$t-1$時刻的狀態以及$t$時刻的輸入預測出$t$時刻的狀態，但是此時的狀態還未結合外部測量，因此是不準確的，存在誤差的。那第二步就是結合$t$時刻的觀測，對預測進行修正，得到新的狀態量的置信度，是爲更新。

總體來說，整個貝葉斯濾波過程相當於閉眼走路的過程。我們想要先向前走兩米，於是閉眼開始走，走完後睜眼看看自己實際走了多少。閉眼走即爲控制，睜眼看即爲觀測。控制過程使得狀態不確定度增大，觀測則將不確定度減小。因此，所謂的濾波指的就是通過測量數據將僅由控制數據進行狀態估計而帶來的不斷提高的噪聲（不確定性）濾掉。

OK，到這裏相信大家對貝葉斯濾波有了直觀的認識。之後所要介紹的卡爾曼濾波及其變體、粒子濾波及其變體本質上都是由貝葉斯濾波變化而來。這部分的知識還涉及了不少的數學理論推導，我在做項目初期曾花費了大量時間去搞清楚這些名詞、理論的來源，對後期理解並編寫代碼起到了很大幫助。不過我在這裏就不贅述了，畢竟這是一個實踐爲主的項目稿，但是每一個算法的最後我都會貼出對我當時研究有很大幫助的文章，正是這些大神分享了他們的學習經驗，才使得這些複雜深奧的概念理解起來如此清晰易懂。

概率機器人的模型及濾波理論
詳解貝葉斯濾波，還包含很多概率論的公式，對忘記公式又懶得查的同學特別友好

3.無跡卡爾曼濾波

可以說沒有一個算法能夠完全適配實際問題，或多或少都需要進行修改，以適應實際需要，卡爾曼濾波也不例外。這篇文章目前看過講卡爾曼濾波最通俗易懂的文章講述了由貝葉斯濾波過渡到卡爾曼濾波的過程，實際上就是給問題加上了一個服從高斯分佈的假設，但是，憑啥你說它符合高斯分佈它就符合啊。事實上，機器人定位這個數學模型確實也不符合高斯線性分佈，那如何去擬合這非線性的模型呢，這時候，無跡卡爾曼濾波（Unscented Kalman Filter）應運而生。

同樣，我們還是先從這個算法的名字說起。“無跡”這個名字讓人看了一頭霧水，濾波濾的軌跡都沒了？非也！無跡卡爾曼濾波是將一種叫“無跡變換（unscented transform）”的非線性變換融入卡爾曼濾波而產生的一種衍生物，而這個無跡變換據說是作者隨便取的，因此可憐了這個偉大的算法也只能跟着叫這個令人摸不着頭腦的名字了。不過無跡卡爾曼濾波的發明人Rudolph van der Merwe（和無跡變換的發明人是兩個人）給這個算法命名爲“Sigma-Point Kalman Filter”（SPPF）倒是更貼切一些。不過後來不知怎地，還是叫無跡卡爾曼濾波更多一些，下文便以此統一縮寫作UKF了。

UKF自誕生起就是爲了解決那些KF、甚至EKF（擴展卡爾曼濾波）解決不了的問題的。它的核心武器UT變換爲其提供了有力支撐。和EKF不同的是，UKF並沒有想着如何去近似非線性的運動和觀測方程，而是使用了一批精心挑選的點去表徵狀態特性，這些點同樣服從高斯分佈。這些點完美地抓住了後驗分佈的均值和方差，並且誤差能夠精確到二階項（EKF用泰勒展開只能精確到一階）。工程人員是最講實用性的，你這一堆天花亂墜的辭藻吹下來，請問怎麼“精心挑選”這些點啊？別急，下面就來講UT變換。

首先我們考慮一個L維的服從高斯分佈的隨機變量$x$，它的均值是$\overline x$，協方差是$P_x$。有一個高度非線性的觀測方程$y=g(x)$，爲了計算y的統計量，我們從先驗中按如下方式取$2L+1$個點，這些點被稱爲sigma點：$\mathcal{X}_0=\overline x \quad\quad w_0^{(m)}=\frac{\lambda}{L+\lambda}\quad\quad i=0$$\mathcal{X}_i=\overline x +(\sqrt {(L+\lambda)P_x})_i \quad i=1,...,L\quad w_0^{(c)}=\frac{\lambda}{L+\lambda}+(1-\alpha^2+\beta)\quad i=0$$\mathcal{X}_i=\overline x -(\sqrt {(L+\lambda)P_x})_i\quad i=L+1,...,2L\quad w_i^{(m)}=w_i^{(c)}=\frac{1}{2(L+\lambda)}\quad i=1,...,2L$
這裏$\mathcal{X}$是sigma點集，裏面共有采樣到的$2L+1$個點，其中第一個點直接就取其先驗均值$\overline x$，隨後的$2L$個點則是按均值對稱分佈的。同時，每一個sigma點還有一個權重$w$，按照距離均值中心遠近逐漸減小。其中$\lambda=\alpha^2(L+\kappa)-L$，$\alpha$、$\beta$、$\kappa$都是參數。說了這麼多，還是不如圖來得清晰，那就直接進入大家最喜歡的“看圖說話環節”

這是一幅在卡爾曼濾波領域引用很多的經典圖片。從左至右分別是蒙特卡洛採樣（粒子濾波，稍後講）、EKF和UKF。這裏我們主要看最右側的這一列，圖中紅色的小圓圈就是通過剛纔那堆算式計算得出的sigma點，它的特點是永遠是奇數個並圍繞均值中心對稱分佈。外面的那層黑色大圈就是協方差。經過一個變換函數$f$，生成了新的sigma點以及均值和協方差，可以看出，估計得到的綠色均值和協方差圈和真實值之間幾乎沒有偏差。

我們從直觀上了解了UT變換，這就足夠了。至於其背後涉及的爲何這種非線性變換能夠有效，權重參數如何選取等問題就不必深究了。完成了sigma點的計算，之後就又迴歸卡爾曼濾波的經典套路，帶入運動方程和觀測方程進行預測、更新，只是在每一次循環的開始要重新計算一遍sigma點。整個UKF的算法流程如下（純手工打造，排版有點醜。。）

4.仿真檢驗

捋清了UKF的算法思路，下一步就是代碼實現了，但一般在直接上手擼C++代碼之前，我們會先用Matlab或是Python這些腳本語言先把算法實現一遍，看指標是否可行，這個步驟在英文中稱爲Prototyping。這裏我選擇了比較熟悉的MATLAB來實現UKF。

既然是概率濾波方法，那首先就得有一系列數據，當然，爲了體現UKF的獨特性能，我們特地選取一些非線性的隨機樣本點，生成樣本點的代碼如下：

%%
%生成隨機樣本
xt=1;
yt=[];
for t=1:T
    current_x=1+sin((4*exp(1)-2)*pi*t)+0.5*xt(t)+gamrnd(3,2);
    xt=[xt;current_x];
    if t<=30
        current_y=0.2*xt(t)^2+normrnd(0,0.00001);
    else
        current_y=0.5*xt(t)-2+normrnd(0,0.00001);
    end
    yt=[yt;current_y];
end

這裏計算current_x和current_y的算式其實就是系統的運動方程$f( , )$和觀測方程$h( , )$

function [x_current] = f(x_previous, v_previous, t_previous)
% Description of the function
% Input:
% x_previous -- row-vector of the previous x-values
% t_previous -- the previous timestep
% Output:
% x_current -- row-vector of the current x-values

% Predefine process parameters
omega = 4*exp(1)-2;
phi1 = 0.5;

% Define helping parameters
n_part = size(x_previous,2);
sin_term = sin(omega*pi*t_previous);

x_current = ones(1,n_part) + repmat(sin_term,1,n_part) + phi1.*x_previous + v_previous;

end

function [ y_predict ] = h( x_value, n, t)

if t <= 30
    y_predict = 0.2 * x_value.^(2) + n;
else
    y_predict = 0.5 * x_value - 2 + n;

end

UKF的核心——如何求取sigma點：

function [sigma_points, sigma_weights, number_of_points] = getSigmaPoints(x_mean_previous_a, P_previous_a, alpha, beta, kappa)
% the Scaled Unscented transformation
% Input:
    % x_mean_previous_a     
    % P_previous_a 
    % alpha
    % beta
    % kappa
% Output:
    % sigma_points          -- dim(3x7)
    % sigma_weights         -- dim(1x7)
    % number_of_points      

n_x_mean_a = size(x_mean_previous_a,1); %3
number_of_points = n_x_mean_a*2 + 1; %7

lambda = alpha^2*(n_x_mean_a + kappa)-n_x_mean_a; 

sigma_points = zeros(n_x_mean_a, number_of_points); %3x7
sigma_weights = zeros(1, number_of_points); %1x7

% Calculate matrix square root 
sqrt_matrix = (chol((n_x_mean_a+lambda)*P_previous_a))'; % 3x3
% Define the sigma_points columns
sigma_points = [zeros(size(P_previous_a,1),1), -sqrt_matrix, sqrt_matrix]; %%%%% 
% Add mean to the rows
sigma_points = sigma_points + repmat(x_mean_previous_a, 1, number_of_points);

% Define the sigma_weights columns 
sigma_weights = [lambda, 0.5*ones(1,(number_of_points-1)), 0] / (n_x_mean_a+lambda);

sigma_weights(number_of_points+1) = sigma_weights(1) + (1-(alpha^2)+beta); 

end

這裏可以看到對之前所說三個參數$\alpha$、$\beta$、$\kappa$是如何使用的。由於我們有三個狀態變量需要預測，因此維度L=3，那麼sigma點數就是$2L+1=7個$。這裏在解決對協方差矩陣求根號時使用了Cholesky分解，可以減少很多運算量，因爲只要對分解出的對角陣求根即可。我的項目導師也提了一個小意見，爲了確保矩陣是半正定的，最好使用SVD分解。大家之後也可以試一試。

再之後的部分就和普通的卡爾曼濾波器幾乎一致了。目的就是預測並更新下一個時刻的均值和方差。

 x_sigma_points = f_function(sigma_points(1:states,:), sigma_points(states+1:states+vNoise,:), t);
    y_sigma_points = h_function(x_sigma_points, sigma_points(states+vNoise+1:states+noises,:), t);

    x_mean_pred = sum(W_x .* (x_sigma_points(:,2:number_of_sigma_points) - repmat(x_sigma_points(:,1),1,number_of_sigma_points-1)),2);
    y_mean_pred = sum(W_y .* (y_sigma_points(:,2:number_of_sigma_points) - repmat(y_sigma_points(:,1),1,number_of_sigma_points-1)),2);

    x_mean_pred = x_mean_pred + x_sigma_points(:,1);
    y_mean_pred = y_mean_pred + y_sigma_points(:,1);

    x_diff = x_sigma_points(:,1) - x_mean_pred;
    y_diff = y_sigma_points(:,1) - y_mean_pred;

    P_pred = sigma_weights(number_of_sigma_points+1)*x_diff*x_diff';
    P_xy = sigma_weights(number_of_sigma_points+1)*x_diff*y_diff';
    P_yy = sigma_weights(number_of_sigma_points+1)*y_diff*y_diff';

    x_diff = x_sigma_points(:,2:number_of_sigma_points) - repmat(x_mean_pred,1,number_of_sigma_points-1);
    y_diff = y_sigma_points(:,2:number_of_sigma_points) - repmat(y_mean_pred,1,number_of_sigma_points-1);
    P_pred = P_pred + (W_x .* x_diff) * x_diff';
    P_yy = P_yy + (W_y .* y_diff) * y_diff';
    P_xy = P_xy + x_diff * (W_y .* y_diff)';

    K = P_xy / P_yy;

    x_est = x_mean_pred + K*( y_true - y_mean_pred);

    P_est = P_pred - K*P_yy*K';

將此估計值同實際值進行比較，能發現吻合度還是相當高的，這也驗證了該方法在非線性環境下的有效性。

5.總結

本節到此就結束了，下一篇文章我會繼續分析定位問題領域的另一大方法——粒子濾波，並將二者進行比較，同時結合RoboCup中的實際難點指出這兩者都不能直接使用的原因。在此基礎上開發出新的改進算法，並部署在機器人上。

參考文獻：
Thrun S. Probabilistic robotics[J]. Communications of the ACM, 2002, 45(3): 52-57.
Van Der Merwe R. Sigma-point Kalman filters for probabilistic inference in dynamic state-space models[D]. OGI School of Science & Engineering at OHSU, 2004.