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

摘要

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

主权项

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系的方向余弦矩阵。