[发明专利]基于RFID和MEMS惯性技术的人员定位方法

权利要求书

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表示系统矩阵的离散化近似项；

F k - 1 = F p p I 3 × 3 0 0 0 F v p F v v ( f c × ) C b c 0 0 0 F ψ ψ 0 - C b c 0 0 0 0 0 0 0 0 0 0 15 × 15 ; ]]>

其中，

F p p = - ( ω e c c × ) = 0 - λ · sin L L · λ · sin L 0 λ · cos L - L · - λ · cos L 0 ; ]]>

F v p = - ω s 2 0 0 0 - ω s 2 0 0 0 2 ω s 2 , ω s 2 = g R M R N ; ]]>

F v v = - ( 2 ω i e c × + ω e c c × ) = 0 - ( 2 ω e + λ · ) sin L L · ( 2 ω e + λ · ) sin L 0 ( 2 ω · e + λ · ) cos L - L · - ( 2 ω e + λ · ) cos L 0 ; ]]>

F Ψ Ψ = - ( ω i e c × + ω e c c × ) = 0 - ( ω e + λ · ) sin L L · ( ω e + λ · ) sin L 0 ( ω e + λ · ) cos L - L · - ( ω e + λ · ) cos L 0 ; ]]>

式中，L为纬度；RM子午线曲率半径；

RN卯酉圈曲率半径；为b系到c系的方向余弦矩阵。