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

摘要

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

主权项

一种基于脚部惯性传感器的室内定位方法，其特征在于包括如下步骤：步骤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& {\sigma }_{a_k}^2>{\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& {\sigma }_{w_k}^2>{\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}{\Sigma }}{V}_i\left(t\right){\omega }_{\mathrm{ij}}\left(t\right)\\ V_i(t+1)=E_i(t+1)+\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)满足：${\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}{{\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_{\alpha }{(v_x\left(t\right),v_y\left(t\right))}^T\\ R_{\alpha }=\left(\begin{array}{cc}\cos \alpha & -\sin \alpha \\ \sin \alpha & \cos \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}{\Sigma }}{\mathrm{\Delta x}}_k^{G_m}\left(t\right)\cos {\mathrm{\Delta \theta }}_k+\underset{k=1}{\overset{i}{\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 {\theta }_i,\sin {\theta }_i/r_m^{\prime }\left(t\right))\\ {\theta }_i=\underset{k=1}{\overset{i}{\Sigma }}\Delta {\theta }_k\end{array}\right.---\left(22\right);$步骤4、根据步骤3计算得到的地理空间方位改变量和方向角θ_i，结合初始位置为坐标(X₀，Y₀)，绘制行人位置轨迹图，其具体实现如下：利用地理空间方位改变量计算改变距离Δs_i：$\Delta s_i=\sqrt{\Delta X_i^{G_m}{\left(t\right)}^2+\Delta Y_i^{G_m}{\left(t\right)}^2}---\left(23\right)$每个距离变化都对应一个方向角θ_i，因此其第i次轨迹绘制方式如下：