openshoe 算法

概述

openshoe 是一种通过惯性测量元件 (IMU) 来对运动进行积分以得到路径的控制算法。算法的核心是 ZUPT（零速度更新算法），在只有 IMU 惯性测量元件的情况下获得路径只能通过对加速度积分得到速度，再将速度对时间进行积分得到路径，这里如果不能对积分得到的速度进行有效的校正那么这个速度误差会时刻对时间进行积分，导致路径完全失效，因此必须采取有效手段及时对速度误差进行校正以得到较为准确的路径信息，ZUPT 就是一种基于检测零速度进行路径积分校正的算法。该算法的关键是对零速度的准确检测。

零速度检测算法

openshoe 采用广义似然检测算法对零速度进行检测，以判断 IMU 是否处于静止状态。

广义似然检测原理

要理解广义似然检测原理需要先熟悉几个概率论中相关的概念，详细如下：

条件概率公式

设 A 与 B 为样本空间 Ω 中的两个事件，其中 P(B)>0。那么在事件 B 发生的条件下，事件 A 发生的条件概率为：

$$P(A|B)=\frac{P(AB)}{P(B)}$$

贝叶斯公式

$$P(A|B)=\frac{P(B|A)P(A)}{P(B)}$$

其中 A 以及 B 为随机事件，且 P(B) 不为零。P(A|B) 是指在事件 B 发生的情况下事件 A 发生的概率。

全概率公式

若事件 A1，A2，…构成一个完备事件组且都有正概率，则对任意一个事件 B，有如下公式成立：

$$\begin{array}{rl}P(B)& =P(BA1)+P(BA2)+...+P(BAn)\\ & =P(B|A1)P(A1)+P(B|A2)P(A2)+...+P(B|An)P(An)\\ & =\sum _{i=1}^{n}P(A_i)P(B|A_i))\end{array}$$

似然函数

似然函数是一种关于统计模型参数的函数。给定输出 x 时，关于参数θ的似然函数$L(\theta |x)$（在数值上）等于给定参数θ后变量 X 的概率：

$$L(\theta |x)=P(X=x|\theta )$$

最大似然估计

给定一个概率分布$D$，已知其概率密度函数（连续分布）或概率质量函数（离散分布）为$f_D$，以及一个分布参数$\theta$ ，我们可以从这个分布中抽出一个具有$n$个值的采样$X_1,X_2,\dots ,X_n$，利用$f_D$计算出其似然函数：

$${\displaystyle (\theta \mid x_1,\dots ,x_n)=f_{\theta }(x_1,\dots ,x_n).}$$

若 D 是离散分布，${\displaystyle f_{\theta }}$即是在参数为$\theta$ 时观测到这一采样的概率。若其是连续分布，${\displaystyle f_{\theta }}$则为$X_1,X_2,\dots ,X_n$联合分布的概率密度函数在观测值处的取值。一旦我们获得$X_1,X_2,\dots ,X_n$，我们就能求得一个关于$\theta$ 的估计。最大似然估计会寻找关于$\theta$ 的最可能的值（即，在所有可能的$\theta$ 取值中，寻找一个值使这个采样的“可能性”最大化）。从数学上来说，我们可以在$\theta$ 的所有可能取值中寻找一个值使得似然函数取到最大值。这个使可能性最大的$\hat{\theta }$值即称为$\theta$ 的最大似然估计。由定义，最大似然估计是样本的函数。

似然比检测

似然比检验是利用似然函数来检测某个假设是否有效的一种检验。一般情况下，要检测某个附加的参数限制是否是正确的，可以将加入附加限制条件的较复杂模型的似然函数最大值与之前的较简单模型的似然函数最大值进行比较。如果参数限制是正确的，那么加入这样一个参数应当不会造成似然函数最大值的大幅变动。一般使用两者的比例来进行比较，这个比值是卡方分配。 设$L(x_{1:n};\theta )$为似然函数，${\theta }_1{\theta }_2\in \mathrm{\Theta }$, 两个参数 ${\theta }_1,{\theta }_2$下似然函数的比值称为似然比。

$$LR=\frac{L(x_{1:n};{\theta }_1)}{L(x_{1:n};{\theta }_2)}$$

算法实现

for k=1:N-W+1

    ya_m=mean(u(1:3,k:k+W-1),2);

    for l=k:k+W-1
        tmp=u(1:3,l)-g*ya_m/norm(ya_m);
        T(k)=T(k)+u(4:6,l)'*u(4:6,l)/sigma2_g + tmp'*tmp/sigma2_a;
    end
end
T=T./W;

既然这里是采用似然比检验方法，自然要涉及到似然函数的建立与约束条件的选取，广义似然比是对似然比的扩展应用，不依赖于统计量的特性，似然函数和约束需要进行自己建模。对于陀螺仪数据的似然函数就是三轴陀螺仪数据的模的平方，陀螺仪没有重力加速度分量的影响建模比较简单，对于加速度则是先将原始数据进行滑动均值滤波，然后去每个数据与滑动均值滤波的残差的模的平方作为似然函数。而这里的约束就是 IMU 的状态了，我们要判断 IMU 是否是静止，则约束条件就是静止，（IMU 的全状态可能包括 IMU 的各轴平移加旋转的组合）因此采样广义似然比算法是一个比较好的求解约束条件是否成立的方法，我们已知的是 IMU 的结果（状态）反求约束条件是否是静止。以上就是运用广义似然比的建模解析对应的实现就是上面的代码，求解未知约束的似然值与已知约束（IMU 静止）似然值的比值即似然比，如果未知约束的条件与已知约束（IMU 静止）约束相同则似然比就很小接近于 1，否则会很大。通过以上方法可以比较好的判断 IMU 是否处于静止状态，当然结果的好坏取决于 sigma2_g 和 sigma2_a 这两个约束条件为 IMU 静止的似然值的选取，如果这个值不能很好代表静止状态下 IMU 的特征则判断会出现很大误差。

kalman 滤波求解 odometry

预测阶段

更新里程信息

首先是 IMU 姿态更新，openshoe 采用更新四元数的算法进行姿态解算，

$$q_k=\mathrm{\Omega }(w_{k}dt)q_{k-1}$$

$\mathrm{\Omega }$是四元数更新矩阵，$w_k$是角速度，dt 是采样间隔。算法实现如下：

% u(4:6) 为陀螺仪数据
w_tb=u(4:6);

P=w_tb(1)*Ts;
Q=w_tb(2)*Ts;
R=w_tb(3)*Ts;

OMEGA=zeros(4);
OMEGA(1,1:4)=0.5*[0 R -Q P];
OMEGA(2,1:4)=0.5*[-R 0 P Q];
OMEGA(3,1:4)=0.5*[Q -P 0 R];
OMEGA(4,1:4)=0.5*[-P -Q -R 0];

v=norm(w_tb)*Ts;

% 四元数更新方程
if v~=0
    q=(cos(v/2)*eye(4)+2/v*sin(v/2)*OMEGA )*q;
    q=q./norm(q);  %四元数归一化
end

然后更新位置和速度：

$$\begin{gathered} p_k=p_{k-1}+v_{k-1}dt \\ {v}_k=v_{k-1}+(q_{k-1}{f}_k{q}_{k-1}^{\ast }-g)dt \end{gathered}$$

$p_k$代表 k 时刻位置，$v_k$代表 k 时刻速度$f_k$为牵引力，算法实现如下：

% Gravity vector
g_t=[0 0 simdata.g]';

% f_t 是旋转矩阵 x acc 向量，得到瞬时加速度量在 navigation coordinat 坐标系下的表示
f_t=q2dcm(q)*u(1:3);

% Subtract (add) the gravity, to obtain accelerations in navigation
% coordinat system.
acc_t=f_t+g_t;

% State space model matrices
A=eye(6);
A(1,4)=Ts;
A(2,5)=Ts;
A(3,6)=Ts;

B=[(Ts^2)/2*eye(3);Ts*eye(3)];

% Update the position and velocity estimates.
y(1:6)=A*x(1:6)+B*acc_t;

该过程会不断更新 IMU 在世界坐标系下的位置，速度和姿态信息。每一轮数据更新一次，实现里程信息的更新。这里说明一下$q_{k-1}{f}_k{q}_{k-1}^{\ast }$这个公式，这是四元数旋转公式的表达，表示$f_k$按照$q_k-1$这个四元数进行旋转，这里其实就是用 acc 原始数据按照当前 IMU 姿态进行旋转得到世界坐标系下的姿态，而速度更新方程就是$v=v_0+at$这个方程。

更新状态转移矩阵

状态转移矩阵其实就是将上面的更新状态方程写成矩阵的形式，将其中的矩阵系数提取出来，并在每一次循环中更新。下面先写出上面状态更新的矩阵形式：

$$\begin{array}{}\text{(1)}& \left[\begin{array}{c}{p}_k\\ v_k\\ q_k\end{array}\right]=\left[\begin{array}{c}{p}_{k-1}+v_{k-1}dt\\ v_{k-1}+(q_{k-1}{f}_k{q}_{k-1}^{\ast }-g)dt\\ \mathrm{\Omega }(w_{k}dt)q_{k-1}\end{array}\right]=\left[\begin{array}{ccc}I& Idt& O\\ O& I& (q_{k-1}{f}_k{q}_{k-1}^{\ast }-g)q_{k-1}^{\ast }dt\\ O& O& \mathrm{\Omega }(w_{k}dt)\end{array}\right]\left[\begin{array}{c}{p}_{k-1}\\ v_{k-1}\\ p_{k-1}\end{array}\right]\end{array}$$

这里：

$$F=\left[\begin{array}{ccc}I& Idt& O\\ O& I& (q_{k-1}{f}_k{q}_{k-1}^{\ast }-g)q_{k-1}^{\ast }dt\\ O& O& \mathrm{\Omega }(w_{k}dt)\end{array}\right]$$

就是状态转移矩阵，下面我们看看具体的实现。

% Convert quaternion to a rotation matrix
Rb2t=q2dcm(q);

% Transform measured force to force in
% the navigation coordinate system.
f_t=Rb2t*u(1:3);

% Create a ske symmetric matrix of the specific fore vector
St=[0 -f_t(3) f_t(2); f_t(3) 0 -f_t(1); -f_t(2) f_t(1) 0];

% Zero matrix
O=zeros(3);

% Identity matrix
I=eye(3);
...
Fc=[O I O;
    O O St;
    O O O];
    
% Noise gain matrix
Gc=[O O; Rb2t O; O -Rb2t];

% Approximation of the discret time state transition matrices
F=eye(size(Fc))+simdata.Ts*Fc;
G=simdata.Ts*Gc;

为了更清楚的看到结果，这里把这个转移矩阵写为公式形式：

$$F=\left[\begin{array}{ccc}I& O& O\\ O& I& St\ast dt\\ O& O& I\end{array}\right]$$

我们看到这里实现上有一点差异就是这个状态转移矩阵其实是不包括四元数 q 的更新项的，这里这样做其实是很合理的，在我们当前这个系统中四元数的更新是不需要进行卡尔曼滤波的，其实在实现上我们也看到这里并不是通过整个状态转移矩阵直接更新所有状态的而是分拆将每个状态进行更新，我们主要是对位置和速度这两个量进行滤波，实际上也也是只对这两个状态量进行滤波，因此这里状态转移矩阵在跟新前先更新了四元数 q 然后四元数的更新$q=q_{k-1}$就是上面代码中的形式。 在标准卡尔曼滤波中还有一个是测量转移矩阵，由于我们的系统模型中系统状态量是位置 p 和速度 v 这两个量都是不可测量的，只有速度 v 是间接通过 ZUPT 进行估计的因此测量转移矩阵为：

$$H=\left[\begin{array}{ccc}O& I& O\end{array}\right]$$

这里其实并不需要进行什么运算在因此在实现上就省略了。 接着是前验状态协方差矩阵的更新：

$$\begin{array}{}\text{(2)}& \hat{P}=F\ast \hat{P}\ast F^{\prime }+G\ast Q\ast G^{\prime }\end{array}$$

其中 G 为过程噪声转移矩阵，Q 为过程噪声，原始噪声就是加速度跟陀螺仪的噪声，Q 初值的选取是按照加速度和陀螺仪的噪声大小来选取的，系统中选取 0.1 个加速度和 0.1 度每秒作为初始系统误差值。

观测阶段

先说明下整个卡尔曼滤波的建模形式，系统的状态共三个，位置，速度和四元数（四元数可以转化为欧拉角）卡尔曼滤波中更新两个系统量位置和速度（对速度进行滤波间接更新到位置）上文列出了状态更新相关过程，只有上面的过程系统速度误差无法被消除，而速度误差会无时无刻的累加到位置信息上，导致位置信息误差无限扩大最终里程信息不可用。而观测过程就是为了消除速度误差的，观测过程只存在于检测到速度为 0（ZUPT 返回 true）时进行观测过程的更新。 卡尔曼系数的更新：

$$\begin{array}{}\text{(3)}& K=\frac{HP\ast H^{\prime }}{H\ast P\ast H^{\prime }+R}\end{array}$$

H 为单位矩阵 R 为测量误差矩阵都是固定矩阵，分子和分母分别代表了对预测和观测的信任度。 现在有了卡尔曼系数 K 就可以按照对预测和观测的信任度来修正误差了，对于速度误差的修正比较好理解，当前通过陀螺仪积分得到的速度是不为 0 的，而我们通过 ZUPT 检测出来的速度却为 0，我们根据卡尔曼系数来衡量两个数据的可信任程度，然后将 K 作用到现在的积分速度上：

$$\begin{gathered} \begin{array}{}\text{(4)}& v_{err}=-K\ast v \\ v=v+v_{err}\end{array} \end{gathered}$$

用这个来修正速度误差。并更新测量误差矩阵 R，最后是更新后验协方差矩阵 P:

$$\begin{array}{}\text{(5)}& P_k=(I-K\ast H)\ast {\hat{P}}_k\end{array}$$

其实现如下：

K=(P*H')/(H*P*H'+R);
z=-x_h(4:6,k);
        
% Estimation of the perturbations in the estimated navigation
% states
dx=K*z;
% Correct the navigation state using the estimated perturbations. 
% (Subfunction located further down in the file.)
% 使用误差估计进行校正姿态，x_h(:,k) 校正后的姿态，quat 校正后四元数，dx 是误差评估
[x_h(:,k) quat]=comp_internal_states(x_h(:,k),dx,quat);
{
    % Correct the state vector
    x_out=x_in+dx;

    % Correct the rotation matrics
    % dx[7:9] 分量代表了旋转误差估计，OMEGA 则构造了旋转误差转移矩阵，对旋转矩阵 R 进行校正
    epsilon=dx(7:9);
    OMEGA=[0 -epsilon(3) epsilon(2); epsilon(3) 0 -epsilon(1); -epsilon(2) epsilon(1) 0];
    R=(eye(3)-OMEGA)*R;
}
% Update the filter state covariance matrix P.
P=(Id-K*H)*P;

这样整个数据卡尔曼滤波过程就完了，可以看到在 ZUPT 检测到 0 速度前只有一个预测过程，即只更新系统状态和前验系统协方差矩阵，在系统 ZUPT 检测到 0 速度时进行下面的卡尔曼系数，误差校正及后验协方差矩阵的更新，实现对系统速度的最优解算。最终的系统状态的准确性主要取决于 0 速的发生及准确的检测，在人步行的场景下，每一步都会有一个停顿很适合用来检测 0 速，在准确检测到 0 速后速度误差被消除，这样速度误差可以实时得到修正而位置误差仍然会累积，但是这个误差只会在运动状态下累积，对应精度要求不高的场合数据可用。