CN105783923A - 基于rfid和mems惯性技术的人员定位方法 - Google Patents

Abstract

本发明公开了一种基于RFID和MEMS惯性技术的人员定位方法。本发明方法中RFID修正点不仅可以提供人员的姿态和位置、速度信息，还能对MEMS惯导***进行修正，获取陀螺仪和加速度计零偏估计，有助于人员长时间在无GNSS信号环境下工作，实现可靠的外部信息对惯性技术计算信息的精确修正，保证了人员定位的可靠精确；此外，步行者每走一步时均会实施一次零速修正算法，限制位置误差的发散，而为了更准确的检测人员零速时间，本发明还提出了一种基于加权指标的阈值零速检测算法，对常用的几种检测算法的判断指标加权，从而产生更优的指标，与阈值进行比较，对零速的检测准确度更高、结果更可靠。

Description

基于RFID和MEMS惯性技术的人员定位方法

技术领域

本发明涉及一种基于RFID和MEMS惯性技术的人员定位方法。

背景技术

GNSS定位设备发展迅速，市场占有率逐年上升。但由于GNSS易受环境影响的固有不足，使其只适于室外的开阔环境，在室内、丛林以及地下等无GNSS信号的区域无能为力。目前，对于无GNSS信号区域的人员定位，市场上出现了很多方法，主要包括以下两种：一种是基于磁力计和MEMS惯性技术结合的个人导航***，另一种是基于红外技术的感应定位***。基于磁力计和MEMS惯性技术结合的个人导航***，其基本原理是利用MEMS惯性技术进行惯性导航解算，解决了GNSS导航***的缺陷，利用磁力计修正方位信息，但磁力计易受周围磁场的影响，不能有效的抑制方位误差，从而不能实现人员的长时间定位；基于红外技术的感应定位***，其基本原理是将有线连接的红外传感器分布于定位范围之内，被定位者随身携带定位标签，每15秒广播该标签唯一的标识码，被红外传感器感知后根据传感器位置确定定位标签的位置，但其覆盖范围有限，易被干扰，穿透力较差。

此外，在个人定位技术中主要存在如下三种形式：基于运动模型的个人航迹推算定位、个人里程计推算方法定位、基于脚步检测的个人导航***定位。其中，基于运动模型的个人航迹推算定位(PDR，PersonalDead-reckoning)是研究的热点，其定位原理为判断行人迈步，估计步长获得行人移动的距离，通过行人的朝向判断前进的方向，结合距离与方向得到行人相对初始点的相对位移。因此，PDR需要重点解决三个问题，即步数的检测与计数、步长的估计及前进朝向的估计。步数的检测与计数最常见的方法为阈值检测法，检测对象通常为加速度幅值与角速度，或两者同时使用；在某些情况下，磁力计的测量值也可作为检测对象。除了阈值检测以外，峰值检测、过零检测也可作为判断步数的方式，低通滤波则在提高检测精度中起到了重要的作用。此种定位方式的缺点在于人运动时的状态变化非常复杂，很难精确测量。而且人体运动时的振动等会给传感器带来很大噪声，因此，基于运动模型的航迹推算定位技术目前还处于试验和研究阶段，还有很多问题需要解决。

个人里程计推算方法定位的基本原理是将惯性装置安装在腿上的各关节位置，从人体运动学角度对人的步态进行分析。该方法需要复杂的人体步态建模，装置也比较复杂，使用起来不够方便。基于脚步检测的个人导航***包含加速度传感器和移动距离测量装置。其中，加速度传感器用于检测移动方向和与用户相对的重力方向的加速度，并根据检测的结果输出加速度信号；移动距离测量装置用于计算移动方向和重力方向上加速度信号的滑窗求和数据，从而计算出所述滑窗求和数据的和。然后，基于微分的滑窗求和数据检测过零点。将当前过零点和前一过零点之间的检测时间的差值与阈值进行比较，从而基于行走的明显的信号模式检测用户脚步。所述脚步检测设备区分由人体的摆动(例如：振动)引起的过零检测和由用户的脚步引起的过零检测，以便所述脚步检测设备更精确地检测用户脚步。此种定位方式的缺点在于需要根据加速度传感器和距离测量装置短时间能够提供可靠的位置信息，如果人员长时间在无GNSS信号环境工作，那么位置信息将不可靠，而且无法知道载体的航向信息。

发明内容

本发明的目的在于提出一种基于RFID和MEMS惯性技术的人员定位方法，通过行走过程中的零速修正和RFID技术对MEMS惯导***更新的位置、速度、姿态进行修正，利于实现在无GNSS信号环境下精确可靠的人员定位。

为了实现上述目的，本发明采用如下技术方案：

基于RFID和MEMS惯性技术的人员定位方法，包括如下步骤：

a初始化载体，得到载体在T时刻的初始位置、速度和姿态信息；

b利用T时刻估计的陀螺和加速度计零偏对T+1时刻载体的加速度和角速度信息修正；

c将T时刻的载***置、速度和姿态信息及T+1时刻经过零偏修正的载体加速度和角速度信息输入惯导捷联算法，得到更新后T+1时刻载体的位置、速度和姿态信息；

d判断T+1时刻载体是否处于零速状态；如果是，则转到步骤e；如果不是，转到步骤i；

e判断该零速状态位置是否为RFID修正点位置；如果是，则转到步骤f；如果不是，则转到步骤g；

其中，RFID修正点是指每隔一段距离在地面上布置的一个识别点；每个RFID修正点处安装一个RFID标签，用于存储该RFID修正点处的位置和姿态信息；

f将RFID修正点存储的位置、姿态、速度信息与捷联更新后T+1时刻载体的位置、姿态、速度之差作为量测量输入卡尔曼滤波器去估计T+1时刻的状态量，其中，RFID修正点存储的速度值为零；利用T+1时刻估计的位置误差、速度误差、姿态误差补偿T+1时刻载体的位置、速度和姿态信息，同时使用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；转到步骤h；

g将零速与捷联更新后T+1时刻载体的速度差输入卡尔曼滤波器去估计T+1时刻的状态量，利用T+1时刻估计的位置误差、速度误差、姿态误差补偿T+1时刻载体的位置、速度和姿态信息，同时使用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；转到步骤h；

h输出T+1时刻载体的位置、速度和姿态信息；转到步骤j；

i在卡尔曼滤波器中根据T时刻估计的状态量预测T+1时刻估计的状态量，然后利用该T+1时刻估计的位置误差、速度误差、姿态误差状态量对载体T+1时刻的位置、速度和姿态信息进行补偿，同时利用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；

j令T＝T+1，返回执行步骤b。

优选地，步骤d中，零速状态的检测步骤为：

d1由惯导的加速度计和陀螺数据计算广义似然比检测、加速度计测量方差检测、加速度计量测幅值检测及角速度量测能量检测算法的判断指标，分别记为；γ_GLRT,γ_MV,γ_ARE,γ_MAG

d2使用预先设置的k₁，k₂，k₃，k₄对上述四种判断指标加权，得出最优指标γ′，其计算公式为：γ′＝k₁γ_GLRT+k₂γ_MV+k₃γ_ARE+k₄γ_MAG，其中，k₁，k₂，k₃，k₄为各算法分配的权重；

d3将加权后的最优指标γ′与阈值γ相比较，若γ′<γ，则认为载体处于零速状态。

优选地，卡尔曼滤波器的设计如下：

选取惯导的位置误差、速度误差、姿态误差以及加速度计和陀螺仪的常值漂移作为空间模型的状态变量，即：

状态空间方程：X_k＝Φ_k,k-1X_k-1+w_k-1；

式中：V_E、V_N、V_U为导航系三个方向的速度，δV_E、δV_N、δV_U为导航系三个方向的速度误差；L、λ、h为纬度、经度、高程；δL、δλ、δh为纬度、经度、高程误差；为失准角；为加速度计零偏；ε_bx、ε_by、ε_bz为陀螺零偏；状态噪声w_k-1～N(0,Q)；转移矩阵Φ_k,k-1为：Φ_k,k-1≈I+F_k-1T；

式中，T为滤波周期；F_k-1为k-1时刻的***矩阵；I表示单位阵；Φ_k,k-1表示***矩阵的离散化近似项；

$F_{k-1}={\left[\begin{array}{ccccc}{F}_{pp}& I_{3\&times;3}& 0& 0& 0\\ F_{vp}& F_{vv}& (f^c\&times;)& C_b^c& 0\\ 0& 0& F_{\mathrm{\&psi;}\mathrm{\&psi;}}& 0& -C_b^c\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]}_{15\&times;15};$ 其中，

$F_{pp}=-({\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\sin L& \stackrel{\&CenterDot;}{L}\\ \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\sin L& 0& \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\cos L\\ -\stackrel{\&CenterDot;}{L}& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\cos L& 0\end{array}\right];$

$F_{vp}=\left[\begin{array}{ccc}-{\mathrm{\&omega;}}_s^2& 0& 0\\ 0& -{\mathrm{\&omega;}}_s^2& 0\\ 0& 0& 2{\mathrm{\&omega;}}_s^2\end{array}\right],{\mathrm{\&omega;}}_s^2=\frac{g}{\sqrt{R_M{R}_N}};$

$F_{vv}=-(2{\mathrm{\&omega;}}_{ie}^c\&times;+{\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -(2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& \stackrel{\&CenterDot;}{L}\\ (2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& 0& (2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L\\ -\stackrel{\&CenterDot;}{L}& -(2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L& 0\end{array}\right];$

$F_{\mathrm{\&Psi;}\mathrm{\&Psi;}}=-({\mathrm{\&omega;}}_{ie}^c\&times;+{\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& \stackrel{\&CenterDot;}{L}\\ ({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& 0& ({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L\\ -\stackrel{\&CenterDot;}{L}& -({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L& 0\end{array}\right];$

式中，L为纬度；R_M子午线曲率半径；R_N卯酉圈曲率半径；为b系到c系的方向余弦矩阵。

优选地，步骤f中，RFID修正点上的位置、姿态、速度量测方程为：

以RFID标签内存储的位置Pos_RFID、姿态Att_RFID、速度与惯导捷联算法输出的位置Pos_INS、姿态Att_INS、速度Vel_INS之差作为卡尔曼滤波器的量测信息，此时的量测方程为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{Att}}_{RFID}-{\mathrm{Att}}_{INS}\\ {\mathrm{Pos}}_{RFID}-{\mathrm{Pos}}_{INS}\\ 0_{3\&times;1}-{\mathrm{Vel}}_{INS}\end{array}\right]=\left[\begin{array}{c}{H}_{att}\\ H_{pos}\\ H_{vel}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{\mathrm{\&epsiv;}}_{att}\left(t\right)\\ {\mathrm{\&epsiv;}}_{pos}\left(t\right)\\ {\mathrm{\&epsiv;}}_{vel}\left(t\right)\end{array}\right]=H\&CenterDot;X\left(t\right)+\mathrm{\&epsiv;}\left(t\right);$

式中，观测方程H分三部分：H_att、H_pos和H_vel；H_att＝[0_3×30_3×3I_3×30_3×6]，H_pos＝[I_3×30_3×30_3×30_3×6]，H_vel＝[0_3×3I_3×30_3×30_3×6]；ε(t)是***观测噪声矢量，为N(0,R′)的高斯白噪声过程。

优选地，步骤g中，非RFID修正点上的零速更新量测方程为：

Z_k＝H·X_k+η_k；

式中，滤波器观测量Z_k表示k时刻零速与捷联更新速度的差值，即：

$Z_k=\&lsqb;0_{3\&times;1}-V^n\&rsqb;={\left[\begin{array}{ccc}-V_E^n& -V_N^n& -V_U^n\end{array}\right]}^T;$

其中，X_k表示滤波器估计的状态量；η_k是***观测噪声矢量，为N(0,R)的高斯白噪声过程；H＝[0_3×3I_3×30_3×9]为观测矩阵。

本发明具有如下优点：

本发明使用RFID标签存储当前RFID修正点的位置和姿态信息，当步行者精确对准到该RFID修正点上并且脚的方向与预先设置的指示线方向一致时读取RFID标签内的信息，不仅可以提供人员的姿态和位置、速度(零速)信息，还能对MEMS惯导***进行修正，获取陀螺仪和加速度计零偏估计，有助于人员长时间在无GNSS信号环境下工作，实现可靠的外部信息对惯性技术计算信息的精确修正，实现了可靠精确的人员定位；此外，步行者每走一步时均会实施一次零速修正算法，限制位置误差的发散，而为了更准确的检测人员零速时刻，本发明提出了一种基于加权指标的阈值零速检测算法，对常用的几种检测算法的判断指标加权，从而产生更优的指标，与阈值进行比较，对零速的检测准确度更高、结果更可靠。

附图说明

图1为本发明中基于RFID和MEMS惯性技术的人员定位方法的流程示意图。

具体实施方式

本发明的基本思想是利用RFID和MEMS惯性技术相结合，每隔一段距离在地面上铺设一个RFID标签，用来存储此RFID修正点的位置和姿态信息，其中，这些信息可以通过精密测绘仪器精确给定。当人员走到RFID修正点时，会读取修正点内存储的位置和姿态信息，读取的修正点位置、姿态、零速信息与惯导组合，进行位置、姿态和速度量测更新。

下面结合附图以及具体实施方式对本发明作进一步详细说明：

结合图1所示，基于RFID和MEMS惯性技术的人员定位方法，包括如下步骤：

a初始化载体，得到载体在T时刻的初始位置、速度和姿态信息；其中，该姿态信息包括横滚角、俯仰角和航向角信息；

b利用T时刻滤波器估计的陀螺、加速度计零偏对T+1时刻载体的加速度和角速度信息进行修正；

c将T时刻载***置、速度和姿态信息及T+1时刻经过零偏修正的载体加速度和角速度信息输入惯导捷联算法，得到更新后T+1时刻载体的位置、速度和姿态信息；

d判断T+1时刻载体是否处于零速状态；如果是，则转到步骤e；如果不是，转到步骤i；

e判断该零速状态位置是否为RFID修正点位置；如果是，则转到步骤f；如果不是，则转到步骤g；

其中，RFID修正点是指每隔一段距离在地面上布置的一个识别点；每个RFID修正点处安装一个RFID标签，用于存储该RFID修正点处的位置和姿态信息；

在每个RFID修正点处均设有行走方向指示线。

f将RFID修正点存储的位置、姿态、速度与捷联更新后T+1时刻载体的位置、姿态、速度之差作为量测量输入卡尔曼滤波器去估计T+1时刻的状态量，其中，RFID修正点存储的速度值为零；利用T+1时刻估计的位置误差、速度误差、姿态误差补偿T+1时刻载体的位置、速度和姿态信息，同时使用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；转到步骤h；

g将零速与捷联更新后T+1时刻载体的速度差输入卡尔曼滤波器去估计T+1时刻的状态量，利用T+1时刻估计的位置误差、速度误差、姿态误差补偿T+1时刻载体的位置、速度和姿态信息，同时使用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；转到步骤h；

h输出T+1时刻载体的位置、速度和姿态信息；转到步骤j；

i在卡尔曼滤波器中根据T时刻估计的状态量预测T+1时刻估计的状态量，然后利用该T+1时刻估计的位置误差、速度误差、姿态误差状态量对载体T+1时刻的位置、速度和姿态信息进行补偿，同时利用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；

j令T＝T+1，返回执行步骤b。

本发明中载体上安装有惯导，该载体可以位于人员鞋底位置。惯导包括陀螺和加速度计，陀螺输出运载体相对于惯性空间的转动速率，加速度计输出载体的比力信息。

设矢量x_k＝[α_kω_k]^T，采样数为W，n时刻到n+W-1时刻的观测量为和根据假设检验理论，在γ_x<γ的条件下，人员处于静止状态。

γ_x代表GLRT、MV、MAG、ARE四种检测器指标γ_GLRT、γ_MV、γ_ARE、γ_MAG，γ是检测器的阈值。不同的检测算法有不同的统计量表达式，常用的零速检测方法有四种：广义似然比检测(GLRT,Generalizedlikelihoodratiotest)、加速度计测量方差检测(MV,Accelerometermeasurementsvariancetest)、加速度计量测幅值检测(MAG,Accelerometermeasurementsmagnitudetest)、角速度量测能量检测(ARE,Angularratemeasurementenergytest)。

各检测器指标计算公式如下：

①广义似然比检测(GLRT) ${\mathrm{\&gamma;}}_{GLRT}=\frac{1}{W}\underset{k=n}{\overset{n+W-1}{\&Sigma;}}\frac{1}{{\mathrm{\&sigma;}}_a^2}\left|\right|y_k^a-g\frac{{\stackrel{\&OverBar;}{y}}_n^a}{\left|\right|{\stackrel{\&OverBar;}{y}}_n^a\left|\right|}|{|}^2+\frac{1}{{\mathrm{\&sigma;}}_{\mathrm{\&omega;}}^2}|\left|{\stackrel{\&OverBar;}{y}}_n^{\mathrm{\&omega;}}\right|{|}^2;$

②加速度计测量方差检测(MV)

③加速度计量测幅值检测(MAG)

④角速度量测能量检测(ARE)

其中，和分别是加速度计和陀螺测量噪声的方差；||a||²＝a^Ta；(·)^T表示转置；表示样本平均值，即

不同的检测算法会使用不同的公式来计算检测器的判断指标，本发明提出一种基于加权指标的阈值零速检测算法，对这几种检测算法的判断指标加权，从而产生更优的指标，与阈值进行比较，对零速的检测准确度更高，结果更可靠。

具体的，步骤d中零速状态的检测步骤为：

d1由加速度计和陀螺数据计算广义似然比检测、加速度计测量方差检测、加速度计量测幅值检测及角速度量测能量检测算法的判断指标，分别记为γ_GLRT,γ_MV,γ_ARE,γ_MAG；

d2使用预先设置的k₁，k₂，k₃，k₄对上述四种判断指标加权，得出最优指标γ′，其计算公式为：γ′＝k₁γ_GLRT+k₂γ_MV+k₃γ_ARE+k₄γ_MAG，其中，k₁，k₂，k₃，k₄为各算法分配的权重，它们的值可以通过大量的实验获得，对于检测精度较高的指标分配较大的权重，相反分配较小的权重；

d3将加权后的最优指标γ′与阈值γ相比较，若γ′<γ，则认为载体处于零速状态。

本发明中卡尔曼滤波器的更新过程包括两个部分：时间更新和量测更新。

当载体未处于零速状态(即静止状态)时，滤波器只进行时间更新(即状态的传递，利用卡尔曼滤波器状态空间方程将T时刻估计的状态量传递到T+1时刻)，根据时间更新预测T+1时刻估计的状态量，然后对T+1时刻捷联算法更新的载***置、速度和姿态信息以及T+2时刻的加速度计、陀螺数据进行补偿；而当载体处于零速状态时，滤波器进行时间更新和量测更新估计状态量，同时对捷联算法更新后T+1时刻的位置、速度和姿态信息以及T+2时刻加速度计、陀螺数据进行补偿，即每走一步实施一次零速修正，从而使大部分误差源得到抑制，实现了可靠精确的人员定位。具体的，卡尔曼滤波器的设计如下：

选取惯导的位置误差、速度误差、姿态误差以及加速度计和陀螺仪的常值漂移作为空间模型的状态变量：

状态空间方程：X_k＝Φ_k,k-1X_k-1+w_k-1；

式中：V_E、V_N、V_U为导航系三个方向的速度，δV_E、δV_N、δV_U为导航系三个方向的速度误差；L、λ、h为纬度、经度、高程；δL、δλ、δh为纬度、经度、高程误差；为失准角；为加速度计零偏；ε_bx、ε_by、ε_bz为陀螺零偏；状态噪声w_k-1～N(0,Q)；状态转移矩阵Φ_k为：Φ_k,k-1≈I+F_k-1T；

式中，T为滤波周期；F_k-1为k-1时刻的***矩阵；I表示单位阵；Φ_k,k-1表示***矩阵的离散化近似项；

$F_{k-1}={\left[\begin{array}{ccccc}{F}_{pp}& I_{3\&times;3}& 0& 0& 0\\ F_{vp}& F_{vv}& (f^c\&times;)& C_b^c& 0\\ 0& 0& F_{\mathrm{\&psi;}\mathrm{\&psi;}}& 0& -C_b^c\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]}_{15\&times;15};$

其中， $F_{pp}=-({\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\sin L& \stackrel{\&CenterDot;}{L}\\ \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}sinL& 0& \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\cos L\\ -\stackrel{\&CenterDot;}{L}& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\cos L& 0\end{array}\right];$

$F_{vp}=\left[\begin{array}{ccc}-{\mathrm{\&omega;}}_s^2& 0& 0\\ 0& -{\mathrm{\&omega;}}_s^2& 0\\ 0& 0& 2{\mathrm{\&omega;}}_s^2\end{array}\right],{\mathrm{\&omega;}}_s^2=\frac{g}{\sqrt{R_M{R}_N}};$

$F_{vv}=-(2{\mathrm{\&omega;}}_{ie}^c\&times;+{\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -(2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& \stackrel{\&CenterDot;}{L}\\ (2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& 0& (2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L\\ -\stackrel{\&CenterDot;}{L}& -(2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L& 0\end{array}\right];$

$F_{\mathrm{\&Psi;}\mathrm{\&Psi;}}=-({\mathrm{\&omega;}}_{ie}^c\&times;+{\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& \stackrel{\&CenterDot;}{L}\\ ({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& 0& ({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L\\ -\stackrel{\&CenterDot;}{L}& -({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L& 0\end{array}\right];$

其中，L为纬度；R_M子午线曲率半径；R_N卯酉圈曲率半径；为b系到c系的方向余弦矩阵。

步骤f中，RFID修正点上的位置、姿态、速度量测方程为：

以RFID标签内存储的位置Pos_RFID、姿态Att_RFID、速度(零速)与惯导捷联算法输出的位置Pos_INS、姿态Att_INS、速度Vel_INS之差作为卡尔曼滤波器的量测信息，此时的量测方程为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{Att}}_{RFID}-{\mathrm{Att}}_{INS}\\ {\mathrm{Pos}}_{RFID}-{\mathrm{Pos}}_{INS}\\ 0_{3\&times;1}-{\mathrm{Vel}}_{INS}\end{array}\right]=\left[\begin{array}{c}{H}_{att}\\ H_{pos}\\ H_{vel}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{\mathrm{\&epsiv;}}_{att}\left(t\right)\\ {\mathrm{\&epsiv;}}_{pos}\left(t\right)\\ {\mathrm{\&epsiv;}}_{vel}\left(t\right)\end{array}\right]=H\&CenterDot;X\left(t\right)+\mathrm{\&epsiv;}\left(t\right);$

式中，观测方程H分三部分：H_att、H_pos和H_vel；H_att＝[0_3×30_3×3I_3×30_3×6]，H_pos＝[I_3×30_3×30_3×30_3×6]，H_vel＝[0_3×3I_3×30_3×30_3×6]；ε(t)是***观测噪声矢量，为N(0,R′)的高斯白噪声过程。

步骤g中，零速更新量测方程为：

Z_k＝H·X_k+η_k；

式中，滤波器观测量Z_k表示k时刻零速与捷联更新速度的差值，即 $Z_k=\&lsqb;0_{3\&times;1}-V^n\&rsqb;={\left[\begin{array}{ccc}-V_E^n& -V_N^n& -V_U^n\end{array}\right]}^T;$

其中，X_k表示滤波器估计的状态量；η_k是***观测噪声矢量，为N(0,R)的高斯白噪声过程；H＝[0_3×3I_3×30_3×9]为观测矩阵。

惯导捷联算法的过程具体如下：

姿态更新

由三轴方向载体角速度求旋转角σ，并求旋转角的模，ω为陀螺仪测量的载体相对于惯性空间的转动速率：σ＝ω·t，

σ×即σ的斜对称阵： $\mathrm{\&sigma;}\&times;=\left[\begin{array}{ccc}0& -{\mathrm{\&sigma;}}_z& {\mathrm{\&sigma;}}_y\\ {\mathrm{\&sigma;}}_z& 0& -{\mathrm{\&sigma;}}_x\\ -{\mathrm{\&sigma;}}_y& {\mathrm{\&sigma;}}_x& 0\end{array}\right];$

求旋转角的正余弦函数泰勒级数展开式：

$\sin x=\underset{n=1}{\overset{\mathrm{\&infin;}}{\&Sigma;}}\frac{{(-1)}^{n-1}{x}^{2n-1}}{(2n-1)!},\cos x=\underset{n=0}{\overset{\mathrm{\&infin;}}{\&Sigma;}}\frac{{(-1)}^n{x}^{2n}}{\left(2n\right)!};$

将上述展开式带入下式：

$C_{b(k+1)}^{b\left(k\right)}=I+\frac{sin\mathrm{\&sigma;}}{\mathrm{\&sigma;}}\&lsqb;\mathrm{\&sigma;}\&times;\&rsqb;+\frac{(1-cos\mathrm{\&sigma;})}{{\mathrm{\&sigma;}}^2}{\&lsqb;\mathrm{\&sigma;}\&times;\&rsqb;}^2;$

分别求得载体坐标系和导航坐标系t+1时刻相对于t时刻的姿态阵和

最终姿态更新公式为：

$C_{b(k+1)}^{n(k+1)}=C_{n\left(k\right)}^{n(k+1)}{C}_{b\left(k\right)}^{n\left(k\right)}{C}_{b(k+1)}^{b\left(k\right)}.$

速度更新

由该位置的大地坐标和地球基本参数，求导航坐标系下的重力值g(h)：

$g\left(h\right)=g_0\&lsqb;1-\frac{2}{R_e}(1+f+m-2{\mathrm{fsin}}^2\mathrm{\&lambda;})h+\frac{3}{R_e^2}{h}^2\&rsqb;;$

当地地理坐标系中地球的自转角速度：

${\mathrm{\&omega;}}_{ie}^n={\left[\begin{array}{ccc}\mathrm{\&Omega;}\cos L& 0& -\mathrm{\&Omega;}\sin L\end{array}\right]}^T;$

当地理坐标系相对于地球固连坐标系的转动角速率：

${\mathrm{\&omega;}}_{en}^n={\left[\begin{array}{ccc}\frac{v_E}{R_0+h}& \frac{-v_N}{R_0+h}& \frac{-v_E\tan L}{R_0+h}\end{array}\right]}^T;$

由哥氏方程推导出地心地固坐标系下的速度微分方程：

${\stackrel{\&CenterDot;}{v}}_e^n=C_b^n{f}^b-\&lsqb;2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n\&rsqb;\&times;v_e^n+g\left(h\right);$

即速度更新方程为 $v_e^n={\stackrel{\&CenterDot;}{v}}_e^n\&CenterDot;dt=\{C_b^n{f}^b-\&lsqb;2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n\&rsqb;\&times;v_e^n+g\left(h\right)\}\&CenterDot;dt;$

位置更新

Pos(k+1)＝Pos(k)+V(k)*dt。

为载体坐标系(b系)到导航坐标系(n系)的姿态变换矩阵；表示地球坐标系(e系)相对于惯性坐标系(i系)的转动角速度在导航坐标系下的投影；表示导航坐标系相对于地球坐标系的转动角速度在导航坐标系下的投影；f^b为加速度计提供的载体坐标系中比力的测量值；h为载体距离地球表面的高度；f表示椭球的扁率；R_e表示地球长半轴；R₀表示地球半径；Pos(k)表示k时刻载体的位置；Ω表示地球自转角速率。

本发明有效发挥了RFID和MEMS惯性技术的优势，来提供位置、姿态、速度信息，有效的抑制惯导***的发散，使得人员能长时间在无GNSS信号环境下工作而位置不会失效，并且使用了新技术RFID来存储当前点的位置、姿态信息。此外，本发明在定位过程中未使用步态检测装置，不用检测复杂的运动变化，***比较简单、快捷，可提供航向信息，而且基于加权指标的阈值零速检测算法能更准确的检测行进过程中的零速状态。

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。

Claims (5)

1.基于RFID和MEMS惯性技术的人员定位方法，其特征在于，包括如下步骤：

a初始化载体，得到载体在T时刻的初始位置、速度和姿态信息；

b利用T时刻估计的陀螺和加速度计零偏对T+1时刻载体的加速度和角速度信息修正；

c将T时刻的载***置、速度和姿态信息及T+1时刻经过零偏修正的载体加速度和角速度信息输入惯导捷联算法，得到更新后T+1时刻载体的位置、速度和姿态信息；

d判断T+1时刻载体是否处于零速状态；如果是，则转到步骤e；如果不是，转到步骤i；

e判断该零速状态位置是否为RFID修正点位置；如果是，则转到步骤f；如果不是，则转到步骤g；

其中，RFID修正点是指每隔一段距离在地面上布置的一个识别点；每个RFID修正点处安装一个RFID标签，用于存储该RFID修正点处的位置和姿态信息；

f将RFID修正点存储的位置、姿态、速度信息与捷联更新后T+1时刻载体的位置、姿态、速度之差作为量测量输入卡尔曼滤波器去估计T+1时刻的状态量，其中，RFID修正点存储的速度值为零；利用T+1时刻估计的位置误差、速度误差、姿态误差补偿T+1时刻载体的位置、速度和姿态信息，同时使用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；转到步骤h；

g将零速与捷联更新后T+1时刻载体的速度差输入卡尔曼滤波器去估计T+1时刻的状态量，利用T+1时刻估计的位置误差、速度误差、姿态误差补偿T+1时刻载体的位置、速度和姿态信息，同时使用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；转到步骤h；

h输出T+1时刻载体的位置、速度和姿态信息；转到步骤j；

i在卡尔曼滤波器中根据T时刻估计的状态量预测T+1时刻估计的状态量，然后利用该T+1时刻估计的位置误差、速度误差、姿态误差状态量对载体T+1时刻的位置、速度和姿态信息进行补偿，同时利用T+1时刻估计的陀螺、加速度计零偏对T+2时刻惯导的角速度和加速度信息进行反馈校正；

j令T＝T+1，返回执行步骤b。

2.根据权利要求1所述的基于RFID和MEMS惯性技术的人员定位方法，其特征在于，步骤d中，零速状态的检测步骤为：

d1由惯导的加速度计和陀螺数据计算广义似然比检测、加速度计测量方差检测、加速度计量测幅值检测及角速度量测能量检测算法的判断指标，分别记为；γ_GLRT,γ_MV,γ_ARE,γ_MAG

d2使用预先设置的k₁，k₂，k₃，k₄对上述四种判断指标加权，得出最优指标γ′，其计算公式为：γ'＝k₁γ_GLRT+k₂γ_MV+k₃γ_ARE+k₄γ_MAG，其中，k₁，k₂，k₃，k₄为各算法分配的权重；

d3将加权后的最优指标γ′与阈值γ相比较，若γ′<γ，则认为载体处于零速状态。

3.根据权利要求1所述的基于RFID和MEMS惯性技术的人员定位方法，其特征在于，卡尔曼滤波器的设计如下：

选取惯导的位置误差、速度误差、姿态误差以及加速度计和陀螺仪的常值漂移作为空间模型的状态变量，即：

状态空间方程：X_k＝Φ_k,k-1X_k-1+w_k-1；

式中：V_E、V_N、V_U为导航系三个方向的速度，δV_E、δV_N、δV_U为导航系三个方向的速度误差；L、λ、h为纬度、经度、高程；δL、δλ、δh为纬度、经度、高程误差；为失准角；为加速度计零偏；ε_bx、ε_by、ε_bz为陀螺零偏；状态噪声w_k-1～N(0,Q)；转移矩阵Φ_k,k-1为：Φ_k,k-1≈I+F_k-1T；

式中，T为滤波周期；F_k-1为k-1时刻的***矩阵；I表示单位阵；Φ_k,k-1表示***矩阵的离散化近似项；

$F_{k-1}={\left[\begin{array}{ccccc}{F}_{pp}& I_{3\&times;3}& 0& 0& 0\\ F_{vp}& F_{vv}& \left(f^c\&times;\right)& C_b^c& 0\\ 0& 0& F_{\mathrm{\&psi;}\mathrm{\&psi;}}& 0& -C_b^c\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]}_{15\&times;15};$

其中，

$F_{pp}=-({\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\sin L& \stackrel{\&CenterDot;}{L}\\ \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\sin L& 0& \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\cos L\\ -\stackrel{\&CenterDot;}{L}& -\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}\cos L& 0\end{array}\right];$

$F_{vp}=\left[\begin{array}{ccc}-{\mathrm{\&omega;}}_s^2& 0& 0\\ 0& -{\mathrm{\&omega;}}_s^2& 0\\ 0& 0& 2{\mathrm{\&omega;}}_s^2\end{array}\right],{\mathrm{\&omega;}}_s^2=\frac{g}{\sqrt{R_M{R}_N}};$

$F_{vv}=-(2{\mathrm{\&omega;}}_{ie}^c\&times;+{\mathrm{\&omega;}}_{ec}^c\&times;)=\left[\begin{array}{ccc}0& -(2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& \stackrel{\&CenterDot;}{L}\\ (2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& 0& (2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L\\ -\stackrel{\&CenterDot;}{L}& -(2{\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L& 0\end{array}\right];$

$F_{\mathrm{\&Psi;}\mathrm{\&Psi;}}=-\left({\mathrm{\&omega;}}_{ie}^c\&times;+{\mathrm{\&omega;}}_{ec}^c\&times;\right)=\left[\begin{array}{ccc}0& -({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& \stackrel{\&CenterDot;}{L}\\ ({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\sin L& 0& ({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L\\ -\stackrel{\&CenterDot;}{L}& -({\mathrm{\&omega;}}_e+\stackrel{\&CenterDot;}{\mathrm{\&lambda;}})\cos L& 0\end{array}\right];$

式中，L为纬度；R_M子午线曲率半径；R_N卯酉圈曲率半径；为b系到c系的方向余弦矩阵。

4.根据权利要求3所述的基于RFID和MEMS惯性技术的人员定位方法，其特征在于，步骤f中，RFID修正点上的位置、姿态、速度量测方程为：

以RFID标签内存储的位置Pos_RFID、姿态Att_RFID、速度与惯导捷联算法输出的位置Pos_INS、姿态Att_INS、速度Vel_INS之差作为卡尔曼滤波器的量测信息，此时的量测方程为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{Att}}_{RFID}-{\mathrm{Att}}_{INS}\\ {\mathrm{Pos}}_{RFID}-{\mathrm{Pos}}_{INS}\\ 0_{3\&times;1}-{\mathrm{Vel}}_{INS}\end{array}\right]=\left[\begin{array}{c}{H}_{att}\\ H_{pos}\\ H_{vel}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{\mathrm{\&epsiv;}}_{att}\left(t\right)\\ {\mathrm{\&epsiv;}}_{pos}\left(t\right)\\ {\mathrm{\&epsiv;}}_{vel}\left(t\right)\end{array}\right]=H\&CenterDot;X\left(t\right)+\mathrm{\&epsiv;}\left(t\right);$

式中，观测方程H分三部分：H_att、H_pos和H_vel；H_att＝[0_3×30_3×3I_3×30_3×6]，H_pos＝[I_3×30_3×30_3×30_3×6]，H_vel＝[0_3×3I_3×30_3×30_3×6]；ε(t)是***观测噪声矢量，为N(0,R′)的高斯白噪声过程。

5.根据权利要求3所述的基于RFID和MEMS惯性技术的人员定位方法，其特征在于，步骤g中，非RFID修正点上的零速更新量测方程为：

Z_k＝H·X_k+η_k；

式中，滤波器观测量Z_k表示k时刻零速与捷联更新速度的差值，即：

$Z_k=\&lsqb;0_{3\&times;1}-V^n\&rsqb;={\left[\begin{array}{ccc}-V_E^n& -V_N^n& -V_U^n\end{array}\right]}^T;$

其中，X_k表示滤波器估计的状态量；η_k是***观测噪声矢量，为N(0,R)的高斯白噪声过程；H＝[0_3×3I_3×30_3×9]为观测矩阵。