一種應用于個人室內導航系統(tǒng)的相對導航方法
【技術領域】
[0001] 本發(fā)明屬于導航方法，具體涉及一種應用于個人室內導航系統(tǒng)的相對導航方法。
【背景技術】
[0002] 由于衛(wèi)星導航所具有的高精度、低功耗、易攜帶等獨特優(yōu)勢，其在日常生活中的應 用日益廣泛，然而在室內、地下及工作廠房等密閉空間中由于衛(wèi)星信號受遮擋衛(wèi)星用戶機 不能定位而無法滿足用戶的導航需求。
[0003] 個人室內導航系統(tǒng)由于其不依賴衛(wèi)星導航、完全自主的獨特優(yōu)勢而受到越來越多 的重視，W美國為首的歐美國家對該系統(tǒng)的研發(fā)起步較早，但尚未出現(xiàn)貨架產(chǎn)品，為了填補 國內對該技術與產(chǎn)品的研發(fā)空白，對個人室內導航技術進行研究。

【發(fā)明內容】

[0004] 本發(fā)明的目的是針對現(xiàn)有技術的缺陷，提供一種應用于個人室內導航系統(tǒng)的相對 導航方法。
[0005] 本發(fā)明是該樣實現(xiàn)的：一種應用于個人室內導航系統(tǒng)的相對導航方法，包括下述 步驟：
[000引步驟一；初始化
[0007] 對速度、位置、姿態(tài)進行初始化，在第一次計算時使用初始化數(shù)據(jù)，在后續(xù)循環(huán)計 算中使用外部傳感器給出的實時信息；
[0008] 步驟二：慣性相對導航解算
[0009] 計算速度、位置信息；
[0010] 步驟H;保存必要信息
[0011] 保存加速度計說、巧螺敏感信息W及加速度計信息在導航坐標系中投影值 fl. *
[0012] 步驟四：計算投影值積分
[0013] 計算加速度計、巧螺敏感信息均方差W及加速度計信息在導航坐標系中投影值積 分；
[0014] 步驟五；檢測是否靜止
[0015] 當系統(tǒng)同時滿足下述H個條件時，判定系統(tǒng)靜止
[0016] 1)H軸加速度計敏感信息的均方差均小于0. 05m/s'2 ;
[0017] 2)H軸巧螺敏感信息的均方差均小于0.OOlrad/s;
[001引3)-個濾波周期內加速度積分絕對值
均小于O.Olm/s;
[0019] 如果判斷結果是系統(tǒng)處于靜止狀態(tài)，則執(zhí)行下面步驟六，否則執(zhí)行步驟二；
[0020] 步驟六：修正輸出
[0021] 對速度、位置、加速度進行修正，修正后的數(shù)據(jù)作為本方法的輸出，并循環(huán)執(zhí)行步 驟二~步驟六，直到導航結束。
[0022] 如上所述的一種應用于個人室內導航系統(tǒng)的相對導航方法，其中，所述的步驟一 的初始化包括給定初始化數(shù)據(jù)，包括
[0023] a)初始位置；X=0,Y=0,Z=0;
[0024] b)初始速度；Vx=0,Vy=0,Vz=0;
[0025]c)初始姿態(tài)：俯仰角巧cl/! =gsin化/各），滾動角 敵W/ =osin(-X《g.cos(巧6姑說，航向角Yaw=0.0,其中的asin表示反正弦函數(shù)
[0026]其中為1砂內加速度的平均值，計算公式為：
[0027]
[002引其中，.成巧和記:巧是加速度計敏感參數(shù)'冷=以點乂；，.說]的兩個輸出值，該 加速度計敏感參數(shù)由外部的加速度計給出。
[0029] 如上所述的一種應用于個人室內導航系統(tǒng)的相對導航方法，其中，所述的步驟二 慣性相對導航解算，包括
[0030] a)姿態(tài)更新
[0031] 設表征載體姿態(tài)的四元數(shù)；Q= [q。屯Q3]t，四元數(shù)更新的解析式為：
[0033] 其中；1為4X4階單位矩陣；
[0032] (1)
句k至IJk+1時刻巧螺敏感到
的角增量，由下式獲得； 癢中T。= 0. 005s，即導航解算周期，姿態(tài)矩陣 仁:; 的更新計算公式為：
[0034]
[00對b)速度、位置更新
[0036] 相對導航速度更新算法為：
[0037] riK + l)^nK) + fi*T" (3)
[0038] 相對導航位置更新算法為：
[0039] P化+1) =P化)+0. 5*(V(K+1)+V化)）*Tn(4)
[0040] 其中iJ為加速度計敏感信息篇在導航系中投影，V似、P似為K時 刻的速度、位置信息，所述的T。為導航周期，一般取50毫砂。
[0041] 如上所述的一種應用于個人室內導航系統(tǒng)的相對導航方法，其中，所述的步驟六 修正輸出中，使用的卡爾曼濾波系統(tǒng)方程為
[0042] 其中，公式（8)中的第一個方程為系統(tǒng)誤差狀態(tài)方程，具體形式見步驟（6. 1)， X(t)為系統(tǒng)誤差狀態(tài)量X(t) = [SP，5X，（35, ▽，e]T，F(xiàn)(t)為系統(tǒng)狀態(tài)轉移矩陣
W(t)為系統(tǒng)噪聲矢量，g為當?shù)刂亓?加速度，
[0043] 公式（8)中的第二個方程為系統(tǒng)觀測方程，具體形式見步驟（6. 2)， Z=[v, KFJ^為零速觀測量，H= [03x3l3X3 03X3 03X3 03X3]，^t)為觀測噪聲矢量'
[0044] (6.1)系統(tǒng)誤差方程
[0045] a)位置誤差方程
[0046] SP=S￥ (9)
[0047] b)速度誤差方程
[0051]d)加速度計零位誤差方程
[0052]
(域
[0053] e)巧螺漂移誤差方程
[0054] (13)
[005引其中，5p=[SPx5Py5Pz]t、5V=[SVx5Vy5Vz]t為位置、速度誤差，cj5 =[cK為姿態(tài)誤差，VyVJ為機體系加速度計零位誤差，e=[e,ej T為機體系巧螺漂移誤差，
[0056] (6. 2)零速匹配觀測方程
[0057] 在系統(tǒng)檢測到人體處于靜止狀態(tài)下，利用下式求觀測量：
[0058] Z=V-0.0 (14)
[0059] 其中，Z為零速匹配卡爾曼濾波器觀測量，沒而為零速狀態(tài)下濾波周期內速度平均 值，在此設定卡爾曼濾波周期為50ms，即10個導航周期濾波一次，
[0060] 觀測方程：
[0061] sv=z=v-m (巧）。
[0062] 本發(fā)明的效果是：本發(fā)明的方法可W準確有效的解算出實驗人員行進的軌跡，說 明本方法正確、有效。
【附圖說明】
[0063] 附圖1是工作人員進行實驗時行走路線的示意圖。
[0064] 附圖2是利用本方法導航得到的軌跡圖。
【具體實施方式】
[0065] 一種應用于個人室內導航系統(tǒng)的相對導航方法，包括下述步驟：
[006引步驟一；初始化
[0067] 給定初始化數(shù)據(jù)，包括
[0068] d)初始位置；X=0,Y=0,Z=0 ;
[0069] e)初始速度；Vx=0,Vy=0,Vz=0 ;
[0070] f)初始姿態(tài)：俯仰角巧c如=0sin化/幻，滾動角 貨W/ =asin(-I%.cos(巧幻々))），航向角Yaw= 0.0。其中的asin表示反正弦函數(shù)
[0071]其中4為1砂內加速度./;L，./i的平均值，計算公式為：
[007引其中，施(0和./;化)是加速度計敏感參數(shù)
的兩個輸出值，該 加速度計敏感參數(shù)由外部的加速度計給出。初始化之后，在后續(xù)計算過程中使用到的速度、 姿態(tài)、加速度等信息，均由外部相應傳感器給出。
[0073] 步驟二：慣性相對導航解算
[0074]C)姿態(tài)更新
[0075] 設表征載體姿態(tài)的四元數(shù)；Q= [q。屯Q3]t，四元數(shù)更新的解析式為：
[0077] 其中；1為4X4階單位矩陣；
[0076] (1)
為k至IJk+1時刻巧螺敏感到
的角增量，由下式獲得： 其中T。= 0. 005s，即導航解算周期，姿態(tài)矩陣 巧 的更新計算公式為：
[0078]
[0079] 即用公式（1)計算qO~q3,計算出該個周期的qO~q3,繼而得到該個周期的姿 態(tài)矩陣。下個周期的姿態(tài)矩陣再利用該個公式計算。
[0080]d)速度、位置更新
[0081] 相對導航速度更新算法為：
[0082]
(3)
[0083] 相對導航位置更新算法為：
[0084] P化+1) =P化)+0. 5*(V(K+1)+V化)）*Tn(4)
[00財其中4"為加速度計敏感信息錢在導航系中投影：.觀=記，V化)、P似為K時 刻的速度、位置信息。所述的T。為導航周期，一般取50毫砂。
[0086] 步驟H;保存必要信息
[0087] 保存加速度計巧螺敏感信息W及加速度計信息在導航坐標系