CN104197938A - 基于脚部惯性传感器的室内定位方法 - Google Patents

Abstract

本发明一种基于脚部惯性传感器的室内定位方法，使用了一个基于扩展卡尔曼滤波的估计结合仿生的网格细胞路径整合方法的架构，其中定位的位置和方向信息由惯性传感器的数据计算得到，而零速修正使用了多条件判断准则，用于判断脚是否着地，当满足条件时将当时的速度置零并触发卡尔曼滤波估计，修正计算出来的速度、位置信息的非线性误差增长，之后将速度信息作为输入利用网格细胞路径整合方法进一步消除位置信息的漂移误差。

Description

基于脚部惯性传感器的室内定位方法

技术领域

本发明涉及一种基于脚部惯性传感器的室内定位方法。

背景技术

基于位置的服务(LBS)对于人们的生活非常重要。LBS的核心技术是定位，目前室外定位技术有比较成熟的GPS定位和移动通信系统的定位等，但在室内常常面临无线信号衰减、定位信号下降等问题。为了满足人们对室内定位的需求，出现了众多室内定位的技术。

室内定位系统用于提供个人和设备的位置信息。惯性传感器定位使用航迹推算方法可以实现连续无缝的定位，航迹推算方法涉及到三个因素：步数、航向和步长，步数与航向可以通过惯性传感器获得。基于惯性传感器的室内定位系统中，方向的漂移与误差累积是这类定位系统最大的问题。大多数研究机构使用了磁强计等其他传感器、先验的地图信息或者计算量较大的粒子滤波算法用于修正方向的漂移并消除误差累积。但是这些方法都有一些不足：首先，加入磁强计等传感器后，由于磁场易受干扰，这会直接导致定位方向的漂移甚至出错；其次，WIFI、地图定位方法需要预先采集数据，其工作量较大，且定位精度不高；另外，粒子滤波算法的计算量很大，很难满足实时定位的要求；最后，现有类似的直接使用Kalman方法虽然能消除累积误差，但漂移问题无法解决。

发明内容

本发明涉及一种基于脚部惯性传感器的室内定位方法，在使用扩展卡尔曼滤波器去除误差累积后，再利用网格细胞路径整合方法处理定位中漂移。

一种基于脚部惯性传感器的室内定位方法，包括如下步骤：

步骤1、实时采集加速度计和陀螺仪的数据获得行人步行方向信息并进行计步：

(1)步行开始时初始对准，确定定位起点，并通过陀螺仪的初始数据确定初始偏航角；

(2)对加速度计和陀螺仪输出的加速度和角速度进行坐标转换后，分别经过积分得到作为时间k的原始定位信息的状态变量包括位置信息(x_k y_k z_k)、速度信息(v_xk v_yk v_zk)和姿态矩阵(roll_k pith_k heading_k)；

(3)利用加速度计和陀螺仪的输出数据，计算出在时间k的合加速度a_k-total及其方差在时间k的合角加速度w_k-total及其方差根据以下公式(1)至(4)组成的四个零速修正判断条件来判定脚是否着地，以完成一次计步：

$C1=\left\{\begin{array}{cc}1& a_{\min }<\left|a_{k-\mathrm{total}}\right|<a_{\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(1\right)$

$C2=\left\{\begin{array}{cc}1& {\mathrm{\&sigma;}}_{a_k}^2>{\mathrm{\&sigma;}}_{a-\min }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(2\right)$

$C3=\left\{\begin{array}{cc}1& \left|w_{k-\mathrm{total}}\right|<w_{\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(3\right)$

$C4=\left\{\begin{array}{cc}1& {\mathrm{\&sigma;}}_{w_k}^2>{\mathrm{\&sigma;}}_{w-\min }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(4\right)$

其中，a_k-total表示在时间k的合加速度,则是加速度的方差,w_k-total表示在时间k的合角加速度,是在运动方向上角加速度的方差，当且仅当上述四个判断条件都符合，即C1＝C2＝C3＝C4＝1时，判定脚着地，完成一次计步并继续步骤2，否则判定脚在空中，一步未完成，返回步骤(2)；

步骤2、若判定脚着地则触发零速修正条件下的卡尔曼滤波，对步骤(2)中计算出来的时间k的状态变量进行相应修正与补偿，得到更新后的状态变量：

卡尔曼滤波中所用的状态变量包括位置信息(x_k y_k z_k)、速度信息(v_xk v_yk v_zk)和姿态矩阵(roll_k pith_k heading_k)，由步骤(2)计算得到：

${\hat{x}}_k=\left[\begin{array}{ccccccccc}{x}_k& y_k& z_k& v_{\mathrm{xk}}& v_{\mathrm{yk}}& v_{\mathrm{zk}}& {\mathrm{roll}}_k& {\mathrm{pith}}_k& {\mathrm{heading}}_k\end{array}\right]---\left(5\right)$

同时，计算预测状态变量状态变量的误差协方差矩阵P_k|k-1和状态转移矩阵F_k：

${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1|k-1}+w_k---\left(6\right)$

$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k---\left(7\right)$

$F_k=\frac{{\mathrm{df}}_k\left(x\right)}{\mathrm{dx}}|x_{k-1|k-1}---\left(8\right)$

其中，Q_k为高斯过程白噪声w_k的协方差矩阵；状态转移矩阵F_k和测量矩阵H_k分别是状态转移方程f_k(·)和测量方程h_k(·)的导数，由公式(9)和(10)计算可得，状态转移方程f_k(·)是状态变量的一个非线性方程，测量方程h_k(·)则表征了位置和速度信息的函数关系；

$F_k=\left[\begin{array}{ccc}0& I_{3\&times;3}& 0\\ 0& 0& \mathrm{St}\\ 0& 0& 0\end{array}\right]---\left(9\right)$

H_k＝[0 I_3×3 0]   (10)

其中，I_3×3是一个单位矩阵，St是一个对称矩阵，在四元素法中用于表征身体坐标系的角速度，定义如下：

$\mathrm{St}=\left[\begin{array}{ccc}0& -f\left(3\right)& f\left(2\right)\\ f\left(3\right)& 0& -f\left(1\right)\\ -f\left(2\right)& f\left(1\right)& 0\end{array}\right]---\left(11\right)$

其中矩阵f是加速度的坐标转移矩阵：

其中，就是身体坐标系和地球坐标系之间的转换方程，acc表示加速度；

再计算三个中间量：测量余量测量余量协方差S_k和最优卡尔曼增益K_k：

$\left\{\begin{array}{c}{\tilde{y}}_k=z_k-H_k{\tilde{x}}_{k|k-1}\\ S_k=H_k{P}_{k|k-1}{H}_k^T+R_k\\ K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}\end{array}\right.---\left(13\right)$

z_k＝H_kx_k+v_k   (14)

其中，z_k为状态变量的测量值,R_k为零均值正态分布观测噪声v_k的协方差矩阵；

根据以上计算，结合最优卡尔曼增益K_k和测量余量对其进行更新得到当前的状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\hat{y}}_k---\left(15\right)$

P_k|k＝(I-K_kH_k)P_k|k-1   (16)

步骤3、从修正后的状态变量中提取第4、5、6列数据，得到经过修正后的速度信息v(t)＝(v_x(t),v_y(t),v_z(t))^T作为网格细胞路径整合方法的输入，计算后得到新的位置信息，即地理空间方位改变量和方向角θ_i：

A、设多层神经网络总层数为M，尺度参数为l_m，同一网络层的网格细胞空间发放分布可以用Burgess模型计算，对各层网络细胞进行以下计算：

设第i个细胞在t+1时刻的活跃度为V_i(t+1)，则

$\left\{\begin{array}{c}{E}_i(t+1)=V_i\left(t\right)+\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(t\right){\mathrm{\&omega;}}_{\mathrm{ij}}\left(t\right)\\ V_i(t+1)=E_i(t+1)+\mathrm{\&tau;}(\frac{E_i(t+1)}{{<E_j\left(t\right)>}_{j=1}^N}-E_i(t+1))\end{array}\right.---\left(17\right)$

其中，N为细胞总数，ω_ij(t)为相互间联系权值，为各层网络中所有细胞活跃度的均值，参数τ用于决定细胞间联系的强弱，为防止权值出现负值，设定V_i(t+1)≤0时，V_i(t+1)＝0；

B、为了便于计算，此处增设一个均值细胞，用于获得细胞活跃度均值，均值细胞与其他细胞间连接权值均为1，为了模拟位置细胞的活动，采用高斯函数形式的权值，使得相邻细胞的活动得到增强，远距离细胞的活动受到抑制，该高斯函数ω_ij(t)满足：

${\mathrm{\&omega;}}_{\mathrm{ij}}\left(t\right)=\mathrm{Iexp}(-\frac{{\left|\right|p_i\left(t\right)-p_j\left(t\right)\left|\right|}_{\mathrm{tri}}^2}{{\mathrm{\&sigma;}}^2})-T---\left(18\right)$

其中，为第i个细胞在网格中位置，设N_x，N_y分别为x，y方向的细胞个数，并且满足

$p_{i_x}\left(t\right)=[i_x\left(t\right)-\mathrm{mod}(i_y\left(t\right)+\mathrm{1,2})]l_m---\left(19\right)$

$p_{i_y}\left(t\right)=(i_y\left(t\right)-0.5)\sqrt{3}{l}_m/2---\left(20\right)$

随着空间位置的变化，网格细胞神经发放图也将随之在各层网格细胞神经网络中发生相应变化，以更新后状态变量中的速度信息v(t)＝(v_x(t),v_y(t),v_z(t))^T为输入量，网格细胞神经发放图map_m与v(t)的对应关系为：

$\left\{\begin{array}{c}{\mathrm{map}}_m=r_m\left(t\right)R_{\mathrm{\&alpha;}}{(v_x\left(t\right),v_y\left(t\right))}^T\\ R_{\mathrm{\&alpha;}}=\left(\begin{array}{cc}\cos \mathrm{\&alpha;}& -\sin \mathrm{\&alpha;}\\ \sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\end{array}\right)\end{array}\right.---\left(21\right)$

其中，r_m(t)为第m层网格与空间位置的转换比例，l₀为网格尺度l_m对应的空间距离，各网格细胞层l₀相同，由于r_m(t)＝l_m/l₀，则r_m(t)与l_m成正比关系；α∈[0,3π]为网络旋转偏移量；

C、构建完成所有网络后，开始对网络细胞路径的整合，假设初始位置为坐标(X₀，Y₀)所代表的空间位置，其对应的网格细胞神经发放图中心的初始位置为以该细胞为中心，利用高斯函数设置各细胞的初始权值，确定其余细胞的发放；

D、假设网格细胞与空间位置的转换比例为r_m(t)，在M确定时，可通过初始尺度l₁及尺度间隔Δl，利用l_m＝q+(m-1)Δl计算各层的尺度，式中q为常数因子，进而在设定地理空间距离l₀基础上，计算各层与空间位置的转换比例；接着利用上述公式在各网络层依次计算不同时刻的神经细胞连接权值和细胞的活跃度，假设最大活跃细胞在t_i时刻的网络位置变化量为其网络位置与前一时刻位置的相对方向为Δθ_i，由于神经网络中的方位与地理空间方位存在对应的映射关系，因而基于网格细胞路径整合实质是通过计算网格细胞发放位置变化量及方位角变化量Δθ_i计算t_i时刻网格细胞发放位置与在各层神经网络的相对位置，最终可以利用转换比例r_m(t)推算t_i时刻的地理空间方位改变量和方向角θ_i：

$\left\{\begin{array}{c}({\mathrm{\&Delta;x}}_i^{G_m}\left(t\right),{\mathrm{\&Delta;y}}_i^{G_m}\left(t\right))=(x_0^{G_m}+\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{\mathrm{\&Delta;x}}_k^{G_m}\left(t\right)\cos {\mathrm{\&Delta;\&theta;}}_k+\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{\mathrm{\&Delta;y}}_k^{G_m}\left(t\right)\sin {\mathrm{\&Delta;\&theta;}}_k)\\ ({\mathrm{\&Delta;x}}_i^{G_m}\left(t\right),{\mathrm{\&Delta;Y}}_i^{G_m}\left(t\right))={\left|\right|(x_i^{G_m}\left(t\right),y_i^{G_m}\left(t\right))-(x_0^{G_m},y_0^{G_m})\left|\right|}_2(\cos {\mathrm{\&theta;}}_i,\sin {\mathrm{\&theta;}}_i/r_m^{\&prime;}\left(t\right))\\ {\mathrm{\&theta;}}_i=\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{\mathrm{\&theta;}}_k\end{array}\right.---\left(22\right);$

步骤4、根据步骤3计算得到的地理空间方位改变量和方向角θ_i，结合初始位置为坐标(X₀，Y₀)，绘制行人位置轨迹图，其具体实现如下：

利用地理空间方位改变量计算改变距离Δs_i：

$\mathrm{\&Delta;}{s}_i=\sqrt{\mathrm{\&Delta;}{X}_i^{G_m}{\left(t\right)}^2+\mathrm{\&Delta;}{Y}_i^{G_m}{\left(t\right)}^2}---\left(23\right)$

每个距离变化都对应一个方向角θ_i，因此其第i次轨迹绘制方式如下：

本发明一种基于脚部惯性传感器的室内定位方法，使用了一个基于扩展卡尔曼滤波的估计架构，其中定位的位置和方向信息由惯性传感器的数据计算得到，而零速修正(ZUPT)使用了多条件判断准则，用于判断脚是否着地，当满足条件时将当时的速度置零并触发卡尔曼滤波估计，修正计算出来的速度、位置信息的非线性误差增长，之后将速度信息作为输入利用网格细胞路径整合方法进一步消除位置信息的漂移误差。

附图说明

图1为本发明的系统结构框架图；

图2为本发明中零速修正判定流程图；

图3为本发明中卡尔曼滤波动态系统模型图；

图4为本发明中卡尔曼滤波方程计算流程图；

图5为本发明中网格细胞神经网络各层构建流程图；

图6为本发明中路径整合流程图；

图7为本发明中行人轨迹绘制示意图。

以下结合附图和具体实施例对本发明作进一步详述。

具体实施方式

如图1、2所示，本发明一种基于脚部惯性传感器的室内定位方法，具体包括如下步骤：

步骤1、实时采集加速度计(加速度传感器或方向传感器)和陀螺仪的数据获得行人步行方向信息并进行计步：

(1)步行开始时初始对准，确定定位起点，并通过陀螺仪的初始数据确定初始偏航角；

(2)对加速度计和陀螺仪输出的加速度和角速度进行坐标转换后，分别经过积分得到作为时间k的原始定位信息的状态变量包括位置信息(x_k y_k z_k)、速度信息(v_xk v_yk v_zk)和姿态矩阵(roll_k pith_k heading_k)；

(3)利用加速度计和陀螺仪的输出数据，计算出在时间k的合加速度a_k-total及其方差在时间k的合角加速度w_k-total及其方差根据以下公式(1)至(4)组成的四个零速修正(ZUPT)判断条件来判定脚是否着地，以完成一次计步：

$C1=\left\{\begin{array}{cc}1& a_{\min }<\left|a_{k-\mathrm{total}}\right|<a_{\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(25\right)$

$C2=\left\{\begin{array}{cc}1& {\mathrm{\&sigma;}}_{a_k}^2>{\mathrm{\&sigma;}}_{a-\min }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(26\right)$

$C3=\left\{\begin{array}{cc}1& \left|w_{k-\mathrm{total}}\right|<w_{\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(27\right)$

$C4=\left\{\begin{array}{cc}1& {\mathrm{\&sigma;}}_{w_k}^2>{\mathrm{\&sigma;}}_{w-\min }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(28\right)$

其中，a_k-total表示在时间k的合加速度,则是加速度的方差,w_k-total表示在时间k的合角加速度,是在运动方向上角加速度的方差，当且仅当上述四个判断条件都符合，即C1＝C2＝C3＝C4＝1时，判定脚着地，完成一次计步并继续步骤2，否则判定脚在空中，一步未完成，返回步骤(2)；

步骤2、若判定脚着地则触发零速修正(ZUPT)条件下的卡尔曼滤波，对步骤(2)中计算出来的时间k的状态变量进行相应修正与补偿，得到更新后的状态变量：

卡尔曼滤波的任务是估计和修正状态变量的误差，其触发时间为脚着地的瞬间，也就是说当满足零速修正(ZUPT)条件时会触发卡尔曼滤波，其估计状态变量的误差过程分为两个阶段：预测和状态更新，其动态系统模型如图3所示。

卡尔曼滤波中所用的状态变量包括位置信息(x_k y_k z_k)、速度信息(v_xk v_yk v_zk)和姿态矩阵(roll_k pith_k heading_k)，由步骤(2)计算得到：

${\hat{x}}_k=\left[\begin{array}{ccccccccc}{x}_k& y_k& z_k& v_{\mathrm{xk}}& v_{\mathrm{yk}}& v_{\mathrm{zk}}& {\mathrm{roll}}_k& {\mathrm{pith}}_k& {\mathrm{heading}}_k\end{array}\right]---\left(29\right)$

同时，计算预测状态变量状态变量的误差协方差矩阵P_k|k-1和状态转移矩阵F_k：

${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1|k-1}+w_k---\left(30\right)$

$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k---\left(31\right)$

$F_k=\frac{{\mathrm{df}}_k\left(x\right)}{\mathrm{dx}}|x_{k-1|k-1}---\left(32\right)$

其中，Q_k为高斯过程白噪声w_k的协方差矩阵；状态转移矩阵F_k和测量矩阵H_k分别是状态转移方程f_k(·)和测量方程h_k(·)的导数，由公式(9)和(10)计算可得，状态转移方程f_k(·)是状态变量的一个非线性方程，测量方程h_k(·)则表征了位置和速度信息的函数关系；

$F_k=\left[\begin{array}{ccc}0& I_{3\&times;3}& 0\\ 0& 0& \mathrm{St}\\ 0& 0& 0\end{array}\right]---\left(33\right)$

H_k＝[0 I_3×3 0]   (34)

其中，I_3×3是一个单位矩阵，St是一个对称矩阵，在四元素法中用于表征身体坐标系的角速度，定义如下：

$\mathrm{St}=\left[\begin{array}{ccc}0& -f\left(3\right)& f\left(2\right)\\ f\left(3\right)& 0& -f\left(1\right)\\ -f\left(2\right)& f\left(1\right)& 0\end{array}\right]---\left(35\right)$

其中矩阵f是加速度的坐标转移矩阵：

其中，就是身体坐标系和地球坐标系之间的转换方程，acc表示加速度；

再计算三个中间量：测量余量测量余量协方差S_k和最优卡尔曼增益K_k：

$\left\{\begin{array}{c}{\tilde{y}}_k=z_k-H_k{\tilde{x}}_{k|k-1}\\ S_k=H_k{P}_{k|k-1}{H}_k^T+R_k\\ K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}\end{array}\right.---\left(37\right)$

z_k＝H_kx_k+v_k   (38)

其中，z_k为状态变量的测量值,R_k为零均值正态分布观测噪声v_k的协方差矩阵；

根据以上计算，结合最优卡尔曼增益K_k和测量余量对其进行更新得到当前的状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\hat{y}}_k---\left(39\right)$

P_k|k＝(I-K_kH_k)P_k|k-1   (40)

这就是卡尔曼滤波对误差做了估计和修正之后的结果，其卡尔曼滤波方程计算流程如图4所示。

步骤3、从修正后的状态变量(其形式定义如公式5)中提取第4、5、6列数据，得到经过修正后的速度信息v(t)＝(v_x(t),v_y(t),v_z(t))^T作为网格细胞路径整合方法的输入，得到新的位置信息，即地理空间方位改变量和方向角θ_i：

卡尔曼滤波虽然修正了速度、位置信息的非线性误差增长，但并不能很好的修正陀螺仪方向漂移误差，若直接将该位置信息作为输出，则轨迹绘制中将包含漂移误差，因此，本发明以修正后的速度信息作为网格细胞路径整合方法的输入，重新计算位置信息以去除其漂移误差。

网格细胞是动物大脑中的一种细胞，存在于内嗅皮层，具有显著的空间放电特征，并呈现出网格图样的放电结构。它的主要作用是帮助动物和人类认路。网格细胞在特定空间位置发生重复性的放电，多个放电空间相互交叠形成网格节点，连接网格节点便形成以等边三角形为基本结构的网格。模拟网格细胞形成的放电野可以与外部空间旋转同步，且角度相同；空间范围越大，网格节点数量越多。放电野具有4个基本参数：1)间距，即各节点间的距离；2)方向，即网格相对于参考坐标系的旋转角度；3)位相，即网格相对于参考点的位移；4)放电野大小，即空间放电范围。

根据网格细胞路径整合的机制本发明采用一组尺度不断变化的多层神经网络来模拟，各层神经网络使用Burgess构造的振荡干涉神经网络模型实现。其实现由模拟网格细胞神经网络各层构建(如图5)和路径整合(如图6)两部分构成。

A、设多层神经网络总层数为M，尺度参数为l_m，同一网络层的网格细胞空间发放分布可以用Burgess模型计算，对各层网络细胞进行以下计算：

设第i个细胞在t+1时刻的活跃度为V_i(t+1)，则

$\left\{\begin{array}{c}{E}_i(t+1)=V_i\left(t\right)+\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(t\right){\mathrm{\&omega;}}_{\mathrm{ij}}\left(t\right)\\ V_i(t+1)=E_i(t+1)+\mathrm{\&tau;}(\frac{E_i(t+1)}{{<E_j\left(t\right)>}_{j=1}^N}-E_i(t+1))\end{array}\right.---\left(41\right)$

其中，N为细胞总数，ω_ij(t)为相互间联系权值，为各层网络中所有细胞活跃度的均值，参数τ用于决定细胞间联系的强弱，为防止权值出现负值，设定V_i(t+1)≤0时，V_i(t+1)＝0；

B、为了便于计算，此处增设一个均值细胞，用于获得细胞活跃度均值，均值细胞与其他细胞间连接权值均为1，为了模拟位置细胞的活动，采用高斯函数形式的权值，使得相邻细胞的活动得到增强，远距离细胞的活动受到抑制，该高斯函数ω_ij(t)满足：

${\mathrm{\&omega;}}_{\mathrm{ij}}\left(t\right)=\mathrm{Iexp}(-\frac{{\left|\right|p_i\left(t\right)-p_j\left(t\right)\left|\right|}_{\mathrm{tri}}^2}{{\mathrm{\&sigma;}}^2})-T---\left(42\right)$

其中，为第i个细胞在网格中位置，设N_x，N_y分别为x，y方向的细胞个数，并且满足

$p_{i_x}\left(t\right)=[i_x\left(t\right)-\mathrm{mod}(i_y\left(t\right)+\mathrm{1,2})]l_m---\left(43\right)$

$p_{i_y}\left(t\right)=(i_y\left(t\right)-0.5)\sqrt{3}{l}_m/2---\left(44\right)$

随着空间位置的变化，网格细胞神经发放图也将随之在各层网格细胞神经网络中发生相应变化，以更新后状态变量中的速度信息v(t)＝(v_x(t),v_y(t),v_z(t))^T为输入量，网格细胞神经发放图map_m与v(t)的对应关系为：

$\left\{\begin{array}{c}{\mathrm{map}}_m=r_m\left(t\right)R_{\mathrm{\&alpha;}}{(v_x\left(t\right),v_y\left(t\right))}^T\\ R_{\mathrm{\&alpha;}}=\left(\begin{array}{cc}\cos \mathrm{\&alpha;}& -\sin \mathrm{\&alpha;}\\ \sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\end{array}\right)\end{array}\right.---\left(45\right)$

其中，r_m(t)为第m层网格与空间位置的转换比例，l₀为网格尺度l_m对应的空间距离，各网格细胞层l₀相同，由于r_m(t)＝l_m/l₀，则r_m(t)与l_m成正比关系；α∈[0,3π]为网络旋转偏移量；

C、构建完成所有网络后，就开始对网络细胞路径的整合，假设初始位置为坐标(X₀，Y₀)所代表的空间位置，其对应的网格细胞神经发放图中心的初始位置为以该细胞为中心，利用高斯函数设置各细胞的初始权值，确定其余细胞的发放；

D、假设网格细胞与空间位置的转换比例为r_m(t)，在M确定时，可通过初始尺度l₁及尺度间隔Δl，利用l_m＝q+(m-1)Δl计算各层的尺度，式中q为常数因子，进而在设定地理空间距离l₀基础上，计算各层与空间位置的转换比例；接着就可以利用上述公式在各网络层依次计算不同时刻的神经细胞连接权值和细胞的活跃度，假设最大活跃细胞在t_i时刻的网络位置变化量为其网络位置与前一时刻位置的相对方向为Δθ_i，由于神经网络中的方位与地理空间方位存在对应的映射关系，因而基于网格细胞路径整合实质是通过计算网格细胞发放位置变化量及方位角变化量Δθ_i计算t_i时刻网格细胞发放位置与在各层神经网络的相对位置，最终可以利用转换比例r_m(t)推算t_i时刻的地理空间方位改变量和方向角θ_i：

$\left\{\begin{array}{c}({\mathrm{\&Delta;x}}_i^{G_m}\left(t\right),{\mathrm{\&Delta;y}}_i^{G_m}\left(t\right))=(x_0^{G_m}+\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{\mathrm{\&Delta;x}}_k^{G_m}\left(t\right)\cos {\mathrm{\&Delta;\&theta;}}_k+\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{\mathrm{\&Delta;y}}_k^{G_m}\left(t\right)\sin {\mathrm{\&Delta;\&theta;}}_k)\\ ({\mathrm{\&Delta;x}}_i^{G_m}\left(t\right),{\mathrm{\&Delta;Y}}_i^{G_m}\left(t\right))={\left|\right|(x_i^{G_m}\left(t\right),y_i^{G_m}\left(t\right))-(x_0^{G_m},y_0^{G_m})\left|\right|}_2(\cos {\mathrm{\&theta;}}_i,\sin {\mathrm{\&theta;}}_i/r_m^{\&prime;}\left(t\right))\\ {\mathrm{\&theta;}}_i=\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{\mathrm{\&theta;}}_k\end{array}\right.---\left(46\right)$

网格细胞发放位置变化量及方位角变化量Δθ_i还可通过统计网格细胞发放变化得到，若t_i时刻较t_i-1时刻网络变化量为则

$\left\{\begin{array}{c}(I_i^{G_m}\left(t\right),Q_i^{G_m}\left(t\right))=(\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{I}_k^{G_m}\left(t\right),\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{Q}_k^{G_m}\left(t\right))\\ (X_i^{G_m}\left(t\right),Y_i^{G_m}\left(t\right))={\left|\right|(I_i^{G_m}\left(t\right),Q_i^{G_m}\left(t\right))\left|\right|}_2/r_m^{\&prime;}\left(t\right)\\ \mathrm{\&Delta;}{\mathrm{\&theta;}}_i=\arctan (Q_i^{G_m}\left(t\right)/I_i^{G_m}\left(t\right))\\ {\mathrm{\&theta;}}_i=\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{\mathrm{\&theta;}}_k=\arctan (\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{Q}_i^{G_m}\left(t\right)/\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{I}_i^{G_m}\left(t\right))\end{array}\right.---\left(47\right)$

每隔时间T₀，利用低层尺度路径整合的结果调整高层尺度路径整合，提高空间位置的整合精度。

步骤4、如图7所示，根据步骤3计算得到的地理空间方位改变量和方向角θ_i，结合初始位置为坐标(X₀，Y₀)，绘制行人位置轨迹图，其具体实现如下：

利用地理空间方位改变量计算改变距离Δs_i

$\mathrm{\&Delta;}{s}_i=\sqrt{\mathrm{\&Delta;}{X}_i^{G_m}{\left(t\right)}^2+\mathrm{\&Delta;}{Y}_i^{G_m}{\left(t\right)}^2}---\left(48\right)$

每个距离变化都对应一个方向角θ_i，因此其第i次轨迹绘制方式如下：

以上所述，仅是本发明较佳实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (1)

1.一种基于脚部惯性传感器的室内定位方法，其特征在于包括如下步骤：

步骤1、实时采集加速度计和陀螺仪的数据获得行人步行方向信息并进行计步：

(1)步行开始时初始对准，确定定位起点，并通过陀螺仪的初始数据确定初始偏航角；

(2)对加速度计和陀螺仪输出的加速度和角速度进行坐标转换后，分别经过积分得到作为时间k的原始定位信息的状态变量包括位置信息(x_k y_k z_k)、速度信息(v_xk v_yk v_zk)和姿态矩阵(roll_k pith_k heading_k)；

(3)利用加速度计和陀螺仪的输出数据，计算出在时间k的合加速度a_k-total及其方差在时间k的合角加速度w_k-total及其方差根据以下公式(1)至(4)组成的四个零速修正判断条件来判定脚是否着地，以完成一次计步：

$C1=\left\{\begin{array}{cc}1& a_{\min }<\left|a_{k-\mathrm{total}}\right|<a_{\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(1\right)$

$C2=\left\{\begin{array}{cc}1& {\mathrm{\&sigma;}}_{a_k}^2>{\mathrm{\&sigma;}}_{a-\min }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(2\right)$

$C3=\left\{\begin{array}{cc}1& \left|w_{k-\mathrm{total}}\right|<w_{\max }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(3\right)$

$C4=\left\{\begin{array}{cc}1& {\mathrm{\&sigma;}}_{w_k}^2>{\mathrm{\&sigma;}}_{w-\min }\\ 0& \mathrm{otherwise}\end{array}\right.---\left(4\right)$

其中，a_k-total表示在时间k的合加速度,则是加速度的方差,w_k-total表示在时间k的合角加速度,是在运动方向上角加速度的方差，当且仅当上述四个判断条件都符合，即C1＝C2＝C3＝C4＝1时，判定脚着地，完成一次计步并继续步骤2，否则判定脚在空中，一步未完成，返回步骤(2)；

步骤2、若判定脚着地则触发零速修正条件下的卡尔曼滤波，对步骤(2)中计算出来的时间k的状态变量进行相应修正与补偿，得到更新后的状态变量：

卡尔曼滤波中所用的状态变量包括位置信息(x_k y_k z_k )、速度信息(v_xk v_yk v_zk)和姿态矩阵(roll_k pith_k heading_k)，由步骤(2)计算得到：

${\hat{x}}_k=\left[\begin{array}{ccccccccc}{x}_k& y_k& z_k& v_{\mathrm{xk}}& v_{\mathrm{yk}}& v_{\mathrm{zk}}& {\mathrm{roll}}_k& {\mathrm{pith}}_k& {\mathrm{heading}}_k\end{array}\right]---\left(5\right)$

同时，计算预测状态变量状态变量的误差协方差矩阵P_k|k-1和状态转移矩阵F_k：

${\hat{x}}_{k|k-1}=F_k{\hat{x}}_{k-1|k-1}+w_k---\left(6\right)$

$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k---\left(7\right)$

$F_k=\frac{{\mathrm{df}}_k\left(x\right)}{\mathrm{dx}}|x_{k-1|k-1}---\left(8\right)$

其中，Q_k为高斯过程白噪声w_k的协方差矩阵；状态转移矩阵F_k和测量矩阵H_k分别是状态转移方程f_k(·)和测量方程h_k(·)的导数，由公式(9)和(10)计算可得，状态转移方程f_k(·)是状态变量的一个非线性方程，测量方程h_k(·)则表征了位置和速度信息的函数关系；

$F_k=\left[\begin{array}{ccc}0& I_{3\&times;3}& 0\\ 0& 0& \mathrm{St}\\ 0& 0& 0\end{array}\right]---\left(9\right)$

H_k＝[0 I_3×3 0]   (10)

其中，I_3×3是一个单位矩阵，St是一个对称矩阵，在四元素法中用于表征身体坐标系的角速度，定义如下：

$\mathrm{St}=\left[\begin{array}{ccc}0& -f\left(3\right)& f\left(2\right)\\ f\left(3\right)& 0& -f\left(1\right)\\ -f\left(2\right)& f\left(1\right)& 0\end{array}\right]---\left(11\right)$

其中矩阵f是加速度的坐标转移矩阵：

其中，就是身体坐标系和地球坐标系之间的转换方程，acc表示加速度；

再计算三个中间量：测量余量测量余量协方差S_k和最优卡尔曼增益K_k：

$\left\{\begin{array}{c}{\tilde{y}}_k=z_k-H_k{\tilde{x}}_{k|k-1}\\ S_k=H_k{P}_{k|k-1}{H}_k^T+R_k\\ K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}\end{array}\right.---\left(13\right)$

z_k＝H_kx_k+v_k   (14)

其中，z_k为状态变量的测量值,R_k为零均值正态分布观测噪声v_k的协方差矩阵；

根据以上计算，结合最优卡尔曼增益K_k和测量余量对其进行更新得到当前的状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{\hat{y}}_k---\left(15\right)$

P_k|k＝(I-K_kH_k)P_k|k-1   (16)

步骤3、从修正后的状态变量中提取第4、5、6列数据，得到经过修正后的速度信息v(t)＝(v_x(t),v_y(t),v_z(t))^T作为网格细胞路径整合方法的输入，计算后得到新的位置信息，即地理空间方位改变量和方向角θ_i：

A、设多层神经网络总层数为M，尺度参数为l_m，同一网络层的网格细胞空间发放分布可以用Burgess模型计算，对各层网络细胞进行以下计算：

设第i个细胞在t+1时刻的活跃度为V_i(t+1)，则

$\left\{\begin{array}{c}{E}_i(t+1)=V_i\left(t\right)+\underset{j=1}{\overset{N}{\mathrm{\&Sigma;}}}{V}_i\left(t\right){\mathrm{\&omega;}}_{\mathrm{ij}}\left(t\right)\\ V_i(t+1)=E_i(t+1)+\mathrm{\&tau;}(\frac{E_i(t+1)}{{<E_j\left(t\right)>}_{j=1}^N}-E_i(t+1))\end{array}\right.---\left(17\right)$

其中，N为细胞总数，ω_ij(t)为相互间联系权值，为各层网络中所有细胞活跃度的均值，参数τ用于决定细胞间联系的强弱，为防止权值出现负值，设定V_i(t+1)≤0时，V_i(t+1)＝0；

B、为了便于计算，此处增设一个均值细胞，用于获得细胞活跃度均值，均值细胞与其他细胞间连接权值均为1，为了模拟位置细胞的活动，采用高斯函数形式的权值，使得相邻细胞的活动得到增强，远距离细胞的活动受到抑制，该高斯函数ω_ij(t)满足：

${\mathrm{\&omega;}}_{\mathrm{ij}}\left(t\right)=\mathrm{Iexp}(-\frac{{\left|\right|p_i\left(t\right)-p_j\left(t\right)\left|\right|}_{\mathrm{tri}}^2}{{\mathrm{\&sigma;}}^2})-T---\left(18\right)$

其中，为第i个细胞在网格中位置，设N_x，N_y分别为x，y方向的细胞个数，并且满足

$p_{i_x}\left(t\right)=[i_x\left(t\right)-\mathrm{mod}(i_y\left(t\right)+\mathrm{1,2})]l_m---\left(19\right)$

$p_{i_y}\left(t\right)=(i_y\left(t\right)-0.5)\sqrt{3}{l}_m/2---\left(20\right)$

随着空间位置的变化，网格细胞神经发放图也将随之在各层网格细胞神经网络中发生相应变化，以更新后状态变量中的速度信息v(t)＝(v_x(t),v_y(t),v_z(t))^T为输入量，网格细胞神经发放图map_m与v(t)的对应关系为：

$\left\{\begin{array}{c}{\mathrm{map}}_m=r_m\left(t\right)R_{\mathrm{\&alpha;}}{(v_x\left(t\right),v_y\left(t\right))}^T\\ R_{\mathrm{\&alpha;}}=\left(\begin{array}{cc}\cos \mathrm{\&alpha;}& -\sin \mathrm{\&alpha;}\\ \sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\end{array}\right)\end{array}\right.---\left(21\right)$

其中，r_m(t)为第m层网格与空间位置的转换比例，l₀为网格尺度l_m对应的空间距离，各网格细胞层l₀相同，由于r_m(t)＝l_m/l₀，则r_m(t)与l_m成正比关系；α∈[0,3π]为网络旋转偏移量；

C、构建完成所有网络后，开始对网络细胞路径的整合，假设初始位置为坐标(X₀，Y₀)所代表的空间位置，其对应的网格细胞神经发放图中心的初始位置为以该细胞为中心，利用高斯函数设置各细胞的初始权值，确定其余细胞的发放；

D、假设网格细胞与空间位置的转换比例为r_m(t)，在M确定时，可通过初始尺度l₁及尺度间隔Δl，利用l_m＝q+(m-1)Δl计算各层的尺度，式中q为常数因子，进而在设定地理空间距离l₀基础上，计算各层与空间位置的转换比例；接着利用上述公式在各网络层依次计算不同时刻的神经细胞连接权值和细胞的活跃度，假设最大活跃细胞在t_i时刻的网络位置变化量为其网络位置与前一时刻位置的相对方向为Δθ_i，由于神经网络中的方位与地理空间方位存在对应的映射关系，因而基于网格细胞路径整合实质是通过计算网格细胞发放位置变化量及方位角变化量Δθ_i计算t_i时刻网格细胞发放位置与在各层神经网络的相对位置，最终可以利用转换比例r_m(t)推算t_i时刻的地理空间方位改变量和方向角θ_i：

$\left\{\begin{array}{c}({\mathrm{\&Delta;x}}_i^{G_m}\left(t\right),{\mathrm{\&Delta;y}}_i^{G_m}\left(t\right))=(x_0^{G_m}+\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{\mathrm{\&Delta;x}}_k^{G_m}\left(t\right)\cos {\mathrm{\&Delta;\&theta;}}_k+\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}{\mathrm{\&Delta;y}}_k^{G_m}\left(t\right)\sin {\mathrm{\&Delta;\&theta;}}_k)\\ ({\mathrm{\&Delta;x}}_i^{G_m}\left(t\right),{\mathrm{\&Delta;Y}}_i^{G_m}\left(t\right))={\left|\right|(x_i^{G_m}\left(t\right),y_i^{G_m}\left(t\right))-(x_0^{G_m},y_0^{G_m})\left|\right|}_2(\cos {\mathrm{\&theta;}}_i,\sin {\mathrm{\&theta;}}_i/r_m^{\&prime;}\left(t\right))\\ {\mathrm{\&theta;}}_i=\underset{k=1}{\overset{i}{\mathrm{\&Sigma;}}}\mathrm{\&Delta;}{\mathrm{\&theta;}}_k\end{array}\right.---\left(22\right);$

步骤4、根据步骤3计算得到的地理空间方位改变量和方向角θ_i，结合初始位置为坐标(X₀，Y₀)，绘制行人位置轨迹图，其具体实现如下：

利用地理空间方位改变量计算改变距离Δs_i：

$\mathrm{\&Delta;}{s}_i=\sqrt{\mathrm{\&Delta;}{X}_i^{G_m}{\left(t\right)}^2+\mathrm{\&Delta;}{Y}_i^{G_m}{\left(t\right)}^2}---\left(23\right)$

每个距离变化都对应一个方向角θ_i，因此其第i次轨迹绘制方式如下：