[發明專利]基于RFID和MEMS慣性技術的人員定位方法

在審

• 申請號: 201610005009.0
• 申請日: 2016-01-05
• 公開（公告）號: CN105783923A
• 公開（公告）日: 2016-07-20
• 發明（設計）人: 石波;李耀宗;楊東清;程敏;盧秀山;陽凡林
• 申請（專利權）人: 山東科技大學
• 主分類號: G01C21/18
• 分類號: G01C21/18
• 代理公司: 濟南舜源專利事務所有限公司 37205
• 代理人: 朱玉建

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