现今使用的室内定位技术以射频识别和无线局域网技术为主，此类定位技术在室内具有较高的定位精准度，然而高精度的定位水准依赖于前期采集大量的数据，需要在前期花费巨大的人力和物力成本，这一缺点限制了技术更大规模的应用^[1-5]。为解决此技术缺陷，李旭等^[6]将惯性导航技术与微机电系统(MEMS)定位算法相结合设计了一个手机程序用于提高室内定位精度，该定位算法通过粒子滤波降低噪声干扰提高定位精度，然而由于实验所采用的芯片处理能力有限，为适应芯片的处理能力，算法仅采用了次优而非最优的贝叶斯估计算法，因此所设计的算法短距离定位仍具有3~5 m的误差，精度仍需要进一步提高。信息工程大学的朱彩杰^[7]对基于MEMS的室内定位算法进行了误差修正研究，在一定程度上降低了高频噪声对定位精度的影响，但回型路径的误差仍然有1.42 m，精度仍旧具有提升的空间。

调研发现：惯性导航技术在室内定位时具有短时间内的高精度优势，并且具有硬件成本低、导航自主性及环境适应能力强等优点，由于MEMS硬件设备存在测量误差，在计算过程中累积，导致定位不准。卡尔曼滤波算法能有效的降低系统的漂移误差，能使解算的位置漂移降到1%内，但长时间的导航解算的累积误差问题还不能得到有效的解决，这是研究的重点之一。

1 算法流程

在综合考虑应用需求、运行成本、定位精度等因素的基础上提出了一种融合MT2503与多传感器的MEMS室内定位算法，并在智能终端上开发了一套室内定位系统，通过零速修正和卡尔曼滤波对误差进行校正，其算法流程图如图 1所示。最后通过实验证明该算法有效解决了室内复杂环境因素所造成的影响，并具有较高的定位精度，为网络通信与无线定位提供了借鉴与参考。

2 数学模型 2.1 步频分析模型

以个人行进方向为Y轴正方向，同一水平面Y轴左手侧垂直方向为X轴正方向，人体站立轴线为Z轴，向上为正方向。人体正常行走时，加速度在Z轴方向上具有周期性，从抬脚到落脚的过程中，重心升高后再降低，将垂直加速度计置于腰间，加速度计信号输出先增大后减小，加速度计信号通过有限冲激响应滤波器(FIR低通滤波器5 Hz)过滤，之后用加速度计信号与零速时的信号差值做零速修正，抬脚时加速度计输出值由负变为正，落脚时相反，计步模块算法流程图如图 2所示。

2.2 航向确定模型 2.2.1 磁航向角计算

由公式1计算磁力计姿态角：横滚角γ和俯仰角θ；姿态角通过公式2对磁力计测量值进行坐标转换，将其转换到水平面上。

$ \left\{ \begin{array}{l} \theta = \arcsin \left( {\frac{{f_{ibx}^b}}{g}} \right),\\ \gamma = \arctan \left( {\frac{{f_{iby}^b}}{{f_{ibz}^b}}} \right), \end{array} \right. $

(1)

式1中，f_ibx^b、f_iby^b、f_ibz^b分别为加速度计在X轴、Y轴和Z轴(地理坐标系)上的测量值。

公式2中磁力计测量值分别表示为m_x、m_y、m_Z，转换后表示为X_h、Y_h、Z_h，转换公式为

$ \left[ {\begin{array}{*{20}{c}} {{X_h}}\\ {{Y_h}}\\ {{Z_h}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos \theta \sin \theta \sin \gamma \cos \gamma \sin \theta }\\ {0\cos \gamma - \sin \gamma }\\ { - \sin \theta \sin \gamma \cos \theta \cos \gamma \cos \theta } \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{m_x}}\\ {{m_y}}\\ {{m_Z}} \end{array}} \right], $

(2)

转换完成后，利用水平磁力计数据通过公式3计算磁航向，即

$ \varphi = \left\{ \begin{array}{l} {180^ \circ } - \arctan \frac{{{Y_h}}}{{{X_h}}},{X_h} < 0;\\ - \arctan \frac{{{Y_h}}}{{{X_h}}}\;\;\;\;\;\;\;\;,{X_h} > 0,{Y_h} < 0;\\ \;\;\;\;\;\;\;{90^ \circ }\;\;\;\;\;\;\;\;\;\;\;\;\;,{X_h} = 0,{Y_h} > 0;\\ \;\;\;\;\;\;\;{270^ \circ }\;\;\;\;\;\;\;\;\;\;\;,{X_h} = 0,{Y_h} > 0;\\ {360^ \circ } - \arctan \frac{{{Y_h}}}{{{X_h}}},\left. {{X_h} > 0,{Y_h} > 0;} \right) \end{array} \right. $

(3)

$ \left\{ \begin{array}{l} {H_{{\rm{normal}}}} = \sqrt {m_x^2 + m_y^2 + m_z^2} ;\\ {H_i} = \sqrt {m_{x\_i}^2 + m_{y\_i}^2 + m_{z\_i}^2} ;\\ {{\bar \varphi }_{{\rm{compass}}}} = \frac{{{\varphi _1} + {\varphi _2} + \cdots + {\varphi _N}}}{N};\\ \varphi - 2\sigma \le {\varphi _i} \le \bar \varphi + 2\sigma ,0.5 < \left| {\frac{{{H_i}}}{{{H_{{\rm{normal}}}}}}} \right| < 1.5, \end{array} \right. $

(4)

计算磁航向取上一次落脚时刻到此次抬脚时刻区间内的所有航向值为1个序列，H_normal为无磁场干扰时的地磁场强度；H_i 为运动时的地磁场强度；当外部干扰明显时，H_normal和H_i数值相差较大，此时剔除序列中的磁航向，最后对在(φ-2σ，φ +2σ)范围内的航向序列计算平均值，记为φ _compass；φ为序列均值；σ为序列方差。

2.2.2 陀螺仪航向计算

利用陀螺仪确定航向，其中陀螺仪的姿态矩阵为C_nb

$ {\mathit{\boldsymbol{C}}_{nb}} = \left[ {\begin{array}{*{20}{c}} {\cos \theta \cos \varphi }&{\cos \theta \sin \varphi }&{ - \sin \theta }\\ {\sin \gamma \sin \theta \cos \varphi - \cos \gamma \sin \varphi }&{\sin \gamma \sin \theta \sin \varphi + \cos \gamma \cos \varphi }&{\sin \gamma \cos \theta }\\ {\sin \theta \cos \gamma \cos \varphi + \sin \gamma sin\varphi }&{\sin \theta \cos \gamma \sin \varphi - \cos \varphi \sin \gamma }&{\cos \gamma \cos \theta } \end{array}} \right], $

(5)

故姿态角θ、γ、φ为

$ \begin{array}{*{35}{l}} \theta =-\text{arcsin}\left( {{\mathbf{C}}_{nb}}\left( 1,3 \right) \right), \\ \gamma =\arctan \frac{{{\mathbf{C}}_{nb}}\left( 2,3 \right)}{{{\mathbf{C}}_{nb}}\left( 3,3 \right)}, \\ \varphi =\text{arctan}\frac{{{\mathbf{C}}_{nb}}\left( 1,2 \right)}{{{\mathbf{C}}_{nb}}\left( 1,1 \right)}, \\ \end{array} $

(6)

$ {{\bar \varphi }_{gryo}} = \frac{{{\varphi _1} + {\varphi _2} + \cdots + {\varphi _N}}}{N},\varphi - 2\sigma \le {\varphi _N} \le \varphi + 2\sigma , $

(7)

在计算轨迹时，单步仅需一个航向值计算轨迹，1个序列的航向值包括从落脚时刻到紧邻的抬脚时刻所有航向值，该序列中在(φ -2σ，φ +2σ)范围内的航向序列计算平均值，记为φ_gryo；φ为序列均值；σ为序列方差。

当外部磁场干扰明显时引起磁航向与陀螺仪航向的较大差值，此时磁航向的可信度较低，以陀螺仪航向进行后续轨迹计算^[8]；当磁航向与陀螺仪航向的差值在设定的范围内，则以航向均值进行后续轨迹计算。

融合算法如下

$ \left\{ \begin{array}{l} {\varphi _{i\_{\rm{final}}}} = 0.5\left( {{{\bar \varphi }_{{\rm{gryo}}}} + {{\bar \varphi }_{{\rm{compass}}}}} \right),\left| {{{\bar \varphi }_{{\rm{gryo}}}} - {{\bar \varphi }_{{\rm{compass}}}}} \right| \le {45^ \circ },\\ {\varphi _{i\_{\rm{final}}}} = {{\bar \varphi }_{{\rm{gryo}}}},\left| {{{\bar \varphi }_{{\rm{gryo}}}} - {{\bar \varphi }_{{\rm{compass}}}}} \right| > {45^ \circ }。\end{array} \right. $

(8)

2.3 零速修正模型

在地理坐标系中，设测量的重力加速度为f_g，载体坐标系中，X、Y、Z轴上的分量为x_g、y_g、z_g，相对于载体坐标系，地理坐标系的方向余弦矩阵为 Cb_n，建立地理坐标系与载体坐标系中的重力加速度相互转化公式，即公式(9)

$ \left[ {\begin{array}{*{20}{c}} {{x_g}}\\ {{y_g}}\\ {{Z_g}} \end{array}} \right] = \mathit{\boldsymbol{C}}{\mathit{\boldsymbol{b}}_n}\left[ {\begin{array}{*{20}{c}} 0\\ 0\\ {{f_g}} \end{array}} \right], $

(9)

以最大似然估计判断角速度和加速度的幅值是否低于阈值^[9]，判断公式为

$ \frac{1}{n}\sum {\left( {\frac{1}{{\sigma _\alpha ^2}}{{\left\| {{a_k} - g\frac{{{a_k}}}{{{a_k}}}} \right\|}^2} + \frac{1}{{\sigma _w^2}}{{\left\| {{w_k}} \right\|}^2}} \right)} < \gamma , $

(10)

公式(10)中：γ为阈值；n为采样数；σ_2α为加速度计噪音协方差；σ_w²为陀螺仪噪声协方差；w_k为角速度；a_k为加速度。

在公式(10)中，设置合适阈值，低于阈值即为零速时刻，反之为非零速时刻，依次计算出行人运动的所有零速时刻，零速时刻的速度即为速度误差，然后对卡尔曼滤波公式(11)获得的状态增量进行修正，

$ {\mathit{\boldsymbol{X}}_k} = {\mathit{\boldsymbol{F}}_{k/k - 1}}{X_k} + {B_k}{U_k} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{W_{k - 1}}, $

(11)

公式(11)中：X_k为k时刻状态量；F_k/k-1为k时刻状态矩阵；B_kU_k为外部控制模型；Γ_k-1为k-1时刻至k时刻的噪声增益矩阵；W_k-1为k-1时刻的系统噪声。

量测方程为

$ {Z_k} = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{X}}_k} + {V_k}, $

(12)

公式(12)中，k时刻观测矩阵为H_k，k时刻观测噪声为V_k。卡尔曼滤波计算时，高斯白噪声包括2个部分，观测噪声和系统噪声均为高斯白噪声^[10]，无外部控制模型时，零速时刻的速度为速度误差，实际值为系统速度的相反数，即-v_n，可得

$ \delta {{\mathit{\boldsymbol{\hat X}}}_n} = - {\mathit{\boldsymbol{K}}_n}{{\hat v}_n}, $

(13)

公式(13)中，K_n为卡尔曼增益矩阵。

状态信息更新后为

$ {{\mathit{\boldsymbol{\hat X}}}_n} = {\mathit{\boldsymbol{X}}_n} + \delta {{\mathit{\boldsymbol{\hat X}}}_n} $

(14)

此卡尔曼滤波中，系统状态向量为9维向量，其中三维位置、速度、姿态角估计分别为p_nΦ_nv_n，即

$ {\mathit{\boldsymbol{X}}_k} = {\left[ {\mathit{\boldsymbol{p}}_n^{\rm{T}}\mathit{\boldsymbol{v}}_n^{\rm{T}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_n^{\rm{T}}} \right]^{\rm{T}}}, $

(15)

P、Q、R分别为系统状态、状态噪声和测量噪声协方差矩阵，计算公式如式(16),

$ \left\{ \begin{array}{l} \mathit{\boldsymbol{P}} = {\rm{diag}}\left( {{\rm{diag}}\left( {{{10}^{ - 10}}\left( {1,1,1} \right)} \right),{\rm{diag}}\left( {{{10}^{ - 10}}\left( {1,1,1} \right)} \right),{{\left( {{\rm{ \mathsf{ π} }}/180} \right)}^2}\left( {{{0.1}^2},{{0.1}^2},{{0.1}^2}} \right)} \right);\\ \mathit{\boldsymbol{Q}} = {\rm{diag}}\left( {{\rm{diag}}\left( {{{0.5}^2},{{0.5}^2},{{0.5}^2}} \right),{\rm{diag}}\left( {{{\left( {0.5{\rm{ \mathsf{ π} }}} \right)}^2},{{\left( {0.5{\rm{ \mathsf{ π} }}} \right)}^2},{{\left( {0.5{\rm{ \mathsf{ π} }}} \right)}^2}} \right)/180} \right);\\ \mathit{\boldsymbol{R}} = {\rm{diag}}\left( {{{0.1}^2},{{0.1}^2},{{0.1}^2}} \right)。\end{array} \right. $

(16)

由广义似然比检测零速度时刻，卡尔曼滤波进行波形过滤，之后进行零速矫正，可有效降低惯导室内定位算法的累积误差。

2.4 无效振动修正

行人运动时，会产生一定数量的无效震动，从而造成定位差错，低通滤波可以有效降低无效震动的高频噪声影响^[11]。椭圆型滤波^[12]、切比雪夫滤波^[13]及巴特沃斯滤波^[14]是目前应用较广泛的低通滤波，其中，在通频带内巴特沃斯滤波起伏小、稳定性好；在通带内，巴特沃斯滤波无衰减。因此，定位数据中的高频噪声部分通过巴特沃斯滤波去除。

低通滤波模型平方函数^[15]为

$ {\left| {H\left( {j\mathit{\Omega }} \right)} \right|^2} = \frac{1}{{1 + {{\left( {j\mathit{\Omega }/j{\mathit{\Omega }_c}} \right)}^{2N}}}},N = 1,2, \cdots ,L, $

(17)

阶次的确定依据公式(18)计算

$ N \ge \frac{{\lg \sqrt {\left( {{{10}^{0.1{\alpha _s}}} - 1} \right)\left( {{{10}^{0.1{\alpha _p}}} - 1} \right)} }}{{\lg \left( {\frac{{{\mathit{\Omega }_s}}}{{{\mathit{\Omega }_p}}}} \right)}}, $

(18)

公式(18)中：阻带衰减为α_s；通带衰减为α_p；通带截止频率为Ω_p；阻带截止频率为Ω_s。依据一般行人的步伐频率，采用3Hz为截至频率，即阶次N=3，以3阶巴特沃斯滤波计算室内导航定位数据信息。

2.5 位置解算模型

最优化自回归算法的卡尔曼滤波是一个高效的定位信息数据处理模型，在导弹追踪、雷达系统、机器人导航和图形图像处理中具有广泛应用，对定位信息数据的优化和修正效果明显^[16-18]，因此，研究获取的定位信息数据采用卡尔曼滤波模型进行修正。

设测量人员第k步的步长为S_kS_k，航向角为φ_kφ_k，在走完第k步时人的位置坐标为(E_k, N_k, Z_k)(E_k, N_k, Z_k)，则卡尔曼滤波模型中的状态向量为

$ \mathit{\boldsymbol{X}} = {\left[ {ENS\varphi } \right]^{\rm{T}}}, $

(19)

状态方程为

$ \left\{ \begin{array}{l} {E_{k + 1}} = {E_k} + {S_k}\sin {\varphi _k} + {w_k},\\ {N_{k + 1}} = {N_k} + {S_k}\cos {\varphi _k} + {w_n},\\ {S_{k + 1}} = {S_k} + {w_s},\\ {\varphi _{k + 1}} = {\varphi _k} + {w_\varphi }, \end{array} \right. $

(20)

其中，ω_E, ω_n, ω_s, ω_φω_E, ω_n, ω_s, ω_φ表示互相独立的模型噪声，且均满足高斯白噪声。

观测向量为

$ \mathit{\boldsymbol{Z}} = {\left[ {S \cdot \varphi } \right]^{\rm{T}}}, $

(21)

观测方程为

$ \left\{ \begin{array}{l} {S_{{\rm{PDR}}}} = {S_k} + {w_{{\rm{SPDR}}}},\\ {\varphi _{{\rm{PDR}}}} = {\varphi _k} + {w_{\varphi {\rm{PDR}}}}, \end{array} \right. $

(22)

其中ω_SPDR, ω_φPDR为对应观测量的观测噪声，它们也相互独立，且均满足高斯白噪声。则根据扩展卡尔曼滤波原理，该状态转移矩阵为

$ {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k} \approx \frac{{\partial {f_k}}}{{\partial \mathit{\boldsymbol{X}}}} = \left[ {\begin{array}{*{20}{c}} 1&0&{\sin \varphi }&{S \cdot \cos \varphi }\\ 0&1&{\cos \varphi }&{ - S \cdot \sin \varphi }\\ 0&0&1&0\\ 0&0&0&1 \end{array}} \right]。$

(23)

测量矩阵为

$ \mathit{\boldsymbol{H}} = \left( {\begin{array}{*{20}{c}} 0&0&1&0\\ 0&0&0&1 \end{array}} \right), $

(24)

动态噪声矩阵为

$ {\mathit{\boldsymbol{Q}}_k} = \left[ {\begin{array}{*{20}{c}} {\delta _E^2}&0&0&0\\ 0&{\delta _n^2}&0&0\\ 0&0&{\delta _s^2}&0\\ 0&0&0&{\delta _\varphi ^2} \end{array}} \right], $

(25)

其中取δ_E=δ_n=1 m，δ_s=0.1 m/s, δ_φ=5°。

观测噪声矩阵为

$ {\mathit{\boldsymbol{R}}_k} = \left[ {\begin{array}{*{20}{c}} {{{\left( {0.1\;{\rm{m/s}}} \right)}^2}}&0\\ 0&{{{\left( {{5^ \circ }} \right)}^2}} \end{array}} \right]。$

(26)

为测试该文卡尔曼滤波的效果，测试人员佩戴定位终端对直线轨迹进行了实验，图 3(a)中为行人直线运动的陀螺仪定位图形轨迹，从图中可以看出，在测试后期轨迹发生了较大的偏离，图 3(b)为经卡尔曼滤波矫正后的图形轨迹，从图中可直观看出，修正后的轨迹近似为直线。由此可知，卡尔曼滤波在定位算法中起到了非常重要的作用。

3 实验验证与结果分析 3.1 实验方案

实验采用MEMS模块型号为JY901，其中加速度计、陀螺仪和磁力计均为三轴，该模块还融合了气压高度计，共10轴。实验场所为一学校图书馆大厅，实验中按照圆形路径和不规则路径行走3圈，计算3圈数据的平均值。

3.2 结果分析

图 4为经卡尔曼滤波后的，横坐标，纵坐标，步长、航向角、轨迹偏差对比图，由图可知，卡尔曼滤波能够很好地进行滤波评价，削弱噪声、提高测量精度并避免欧拉运动学方程求解的奇点问题。

为了测量该算法的效果，选取了室内环境进行测试，并与文献[6]、文献[7]中提及的算法进行了对比实验，其结果如表 1所示。由表 1可知，本定位基站算法误差约为0.455 m，准确性远高于文献[6]，文献[7]中提及的算法，设计出的系统不仅精度高而且轨迹稳定。

4 结语

为提高惯导室内定位算法的精度并改善其连续性，提出一种融合MT2503与MEMS传感器的惯性导航定位算法，算法以MT2503芯片作为定位终端，并将加速度计传感器、陀螺仪，磁力计等传感器与其进行融合，通过加速度计传感器解算步长、步频、步幅，通过陀螺仪与磁力计来识别定位终端微动偏移量，最后在初始位置上累加定位终端位移得出定位终端实时位置，最后通过零速修正和卡尔曼滤波对误差进行校正。实验结果证实，本方法能够有效的解决MEMS定位算法中存在的导航解算误差累积问题，显著提升MEMS惯导室内定位算法的精度。