[發明專利]一種應用于個人室內導航系統的相對導航方法

在審

• 申請號: 201410130948.9
• 申請日: 2014-04-02
• 公開（公告）號: CN104977001A
• 公開（公告）日: 2015-10-14
• 發明（設計）人: 姜述明;王根;吳亮華;徐海剛;李瑞賢;郭元江
• 申請（專利權）人: 北京自動化控制設備研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16
• 代理公司: 核工業專利中心 11007
• 代理人: 高尚梅;劉昕宇

1.一種應用于個人室內導航系統的相對導航方法，其特征在于，包括下述步驟：?

步驟一：初始化?

對速度、位置、姿態進行初始化，在第一次計算時使用初始化數據，在后續循環計算中使用外部傳感器給出的實時信息；?

步驟二：慣性相對導航解算?

計算速度、位置信息；?

步驟三：保存必要信息?

保存加速度計陀螺敏感信息以及加速度計信息在導航坐標系中投影值

步驟四：計算投影值積分?

計算加速度計、陀螺敏感信息均方差以及加速度計信息在導航坐標系中投影值積分；?

步驟五：檢測是否靜止?

當系統同時滿足下述三個條件時，判定系統靜止?

1）三軸加速度計敏感信息的均方差均小于0.05m/s^2；?

2）三軸陀螺敏感信息的均方差均小于0.001rad/s；?

3）一個濾波周期內加速度積分絕對值均小于0.01m/s；?

如果判斷結果是系統處于靜止狀態，則執行下面步驟六，否則執行步驟二；?

步驟六：修正輸出?

對速度、位置、加速度進行修正，修正后的數據作為本方法的輸出，?并循環執行步驟二～步驟六，直到導航結束。?

2.如權利要求1所述的一種應用于個人室內導航系統的相對導航方法，其特征在于：所述的步驟一的初始化包括給定初始化數據，包括?

a)初始位置：X=0，Y=0，Z=0；?

b)初始速度：Vx=0，Vy=0，Vz=0；?

c)初始姿態：俯仰角滾動角?航向角Yaw＝0.0，其中的asin表示反正弦函數?

其中為1秒內加速度的平均值，計算公式為：?

其中，和是加速度計敏感參數的兩個輸出值，該加速度計敏感參數由外部的加速度計給出。?

3.如權利要求2所述的一種應用于個人室內導航系統的相對導航方法，其特征在于：所述的步驟二慣性相對導航解算，包括?

a)姿態更新?

設表征載體姿態的四元數：Q＝[q₀?q₁?q₂?q₃]^T，四元數更新的解析式為：?

其中：I為4×4階單位矩陣；為k到k+1時刻陀螺敏感到的角增量，由下式獲得：其中T_n＝0.005s，即導航解算周?期，姿態矩陣的更新計算公式為：?

b)速度、位置更新?

相對導航速度更新算法為：?

相對導航位置更新算法為：?

P(K+1)＝P(K)+0.5*(V(K+1)+V(K))*T_n???(4)?

其中為加速度計敏感信息在導航系中投影：V(K)、P(K)為K時刻的速度、位置信息，所述的T_n為導航周期，一般取50毫秒。?

4.如權利要求3所述的一種應用于個人室內導航系統的相對導航方法，其特征在于：所述的步驟六修正輸出中，使用的卡爾曼濾波系統方程為

其中，公式（8）中的第一個方程為系統誤差狀態方程，具體形式見步驟（6.1），X(t)為系統誤差狀態量X(t)＝[δP,δX,φ,▽,ε]^T，F(t)為系統狀態轉移矩陣W(t)為系統噪聲矢量，g為當地重力加速度，?

公式（8）中的第二個方程為系統觀測方程，具體形式見步驟（6.2），?為零速觀測量，H＝[0_3×3?I_3×3?0_3×3?0_3×3?0_3×3]，V(t)為觀測噪聲矢量，?

（6.1）系統誤差方程?

a)位置誤差方程?

b)速度誤差方程?

c)姿態誤差方程?

d)加速度計零位誤差方程?

e)陀螺漂移誤差方程?

其中，δP=[δP_x?δP_y?δP_z]^T、δV=[δV_x?δV_y?δV_z]^T為位置、速度誤差，φ=[φ_x?φ_y?φ_z]^T為姿態誤差，▽=[▽_x?▽_y?▽_z]^T為機體系加速度計零位誤差，ε=[ε_x?ε_y?ε_z]^T為機體系陀螺漂移誤差，

（6.2）零速匹配觀測方程?

在系統檢測到人體處于靜止狀態下，利用下式求觀測量：?

其中，Z為零速匹配卡爾曼濾波器觀測量，為零速狀態下濾波周期內速度平均值，在此設定卡爾曼濾波周期為50ms，即10個導航周期濾波一次，?

觀測方程：?