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;
卡尔曼滤波器的设计如下:
选取惯导的位置误差、速度误差、姿态误差以及加速度计和陀螺仪的常值漂移作为空间模型的状态变量,即:状态空间方程:Xk=Φk,k-1Xk-1+wk-1;
式中:
VE、VN、VU为导航系三个方向的速度,δVE、δVN、δVU为导航系三个方向的速度误差;
L、λ、h为纬度、经度、高程;
δL、δλ、δh为纬度、经度、高程误差;
为失准角;
为加速度计零偏;
εbx、εby、εbz为陀螺零偏;
状态噪声wk-1~N(0,Q);
转移矩阵Φk,k-1为:Φk,k-1≈I+Fk-1T;式中,T为滤波周期;Fk-1为k-1时刻的系统矩阵;I表示单位阵;Φk,k-1表示系统矩阵的离散化近似项;
其中,
式中,L为纬度;RM子午线曲率半径;
RN卯酉圈曲率半径; 为b系到c系的方向余弦矩阵。
2.根据权利要求1所述的基于RFID和MEMS惯性技术的人员定位方法,其特征在于,步骤d中,零速状态的检测步骤为:d1由惯导的加速度计和陀螺数据计算广义似然比检测、加速度计测量方差检测、加速度计量测幅值检测及角速度量测能量检测算法的判断指标,分别记为;γGLRT,γMV,γARE,γMAGd2使用预先设置的k1,k2,k3,k4对上述四种判断指标加权,得出最优指标γ′,其计算公式为:γ'=k1γGLRT+k2γMV+k3γARE+k4γMAG,其中,k1,k2,k3,k4为各算法分配的权重;
d3将加权后的最优指标γ′与阈值γ相比较,若γ′<γ,则认为载体处于零速状态。
3.根据权利要求1所述的基于RFID和MEMS惯性技术的人员定位方法,其特征在于,步骤f中,RFID修正点上的位置、姿态、速度量测方程为:以RFID标签内存储的位置PosRFID、姿态AttRFID、速度与惯导捷联算法输出的位置PosINS、姿态AttINS、速度VelINS之差作为卡尔曼滤波器的量测信息,此时的量测方程为:式中,观测方程H分三部分:Hatt、Hpos和Hvel;Hatt=[03×3 03×3 I3×3 03×6],Hpos=[I3×3
03×3 03×3 03×6],Hvel=[03×3 I3×3 03×3 03×6];ε(t)是系统观测噪声矢量,为N(0,R′)的高斯白噪声过程。
4.根据权利要求1所述的基于RFID和MEMS惯性技术的人员定位方法,其特征在于,步骤g中,非RFID修正点上的零速更新量测方程为:Zk=H·Xk+ηk;
式中,滤波器观测量Zk表示k时刻零速与捷联更新速度的差值,即:其中,Xk表示滤波器估计的状态量;ηk是系统观测噪声矢量,为N(0,R)的高斯白噪声过程;H=[03×3 I3×3 03×9]为观测矩阵。