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.