角信息和速度信息，遞 推出每個(gè)時(shí)刻的行人位置信息，具體步驟如下：
[0108] 利用MEMS傳感器模塊所檢測(cè)的三軸加速度計(jì)、三軸磁力計(jì)和三軸陀螺儀數(shù)據(jù)，估 算出行人的步長(zhǎng)信息cU和航向角信息Θ,，結(jié)合從北斗接收機(jī)模塊直接獲取到的行人位置信 息，即為初始位置Ρ〇(Νο，Εο)，則可以遞推出第η時(shí)刻的行人位置?"(化而），可由下式確定：
[0109]
(6)
[0110]
[0111] 其中，和di為第i次位移方向和位移距離，Nk為第k時(shí)刻的北向位置值，Ek為第k時(shí) 刻的東向位置值;將濾波估計(jì)誤差結(jié)果對(duì)PDR解算出來的位置信息進(jìn)行相應(yīng)坐標(biāo)軸上的校 正。
[0112]三、步長(zhǎng)的估算方法
[0113]在行人航跡推算系統(tǒng)的設(shè)計(jì)中，首先需要明確知道行人每個(gè)時(shí)刻所處的狀態(tài)，本 文基于行人行走的特點(diǎn)將其劃分為兩種狀態(tài):靜止和運(yùn)動(dòng)。根據(jù)加速度計(jì)的特性，行人在靜 止和運(yùn)動(dòng)時(shí)的加速度值相差較大，行人靜止時(shí)，加速度計(jì)雖然仍有輸出，但其變化范圍小并 且峰值明顯小于正常步行時(shí)的加速度峰值。行人行走的步長(zhǎng)是變化值，那么需要考慮到行 人行走步長(zhǎng)變化，步長(zhǎng)系數(shù)k可以直觀反映這種步長(zhǎng)變化，步長(zhǎng)系數(shù)k可以根據(jù)多次測(cè)試獲 取，結(jié)合加速度計(jì)的三軸數(shù)據(jù)，可以實(shí)時(shí)計(jì)算行人步長(zhǎng)信息。定義stride (m)為步長(zhǎng)公式，貝lj 根據(jù)三軸加速度計(jì)數(shù)據(jù)計(jì)算得到的步長(zhǎng)估計(jì)公式具體如下：
[0114]
(7)
[0115] 其中：ax，ay，az分別為三軸加速度的X軸、y軸、2軸輸出， &_和&*分別為三軸加速 度計(jì)輸出加速度模值的最大值max和最小值min，即是從抬腳時(shí)刻到落腳時(shí)刻之間的最大值 和最小值;tup和tdmm對(duì)應(yīng)抬腳up、落腳down時(shí)刻。
[0116] 利用MEMS傳感器模塊所測(cè)量的三軸加速度數(shù)據(jù)、三軸磁力計(jì)數(shù)據(jù)，通過PI控制以 及互補(bǔ)濾波算法對(duì)三軸陀螺儀數(shù)據(jù)進(jìn)行校正（由于陀螺儀短時(shí)精度高，長(zhǎng)時(shí)間工作會(huì)引起 漂移，而對(duì)于加速度計(jì)和磁力計(jì)，它們短時(shí)間內(nèi)精度沒有陀螺儀高，但長(zhǎng)時(shí)間內(nèi)卻能保持穩(wěn) 定，那么可以對(duì)陀螺儀輸出數(shù)據(jù)補(bǔ)償。通過分析九軸傳感器數(shù)據(jù)，從頻域上陀螺儀顯示高通 特性，而加速度計(jì)與磁力計(jì)顯示低通特性，因此它們?cè)陬l域上可以相互補(bǔ)充。），具體校正過 程如下：
[0117] (a)互補(bǔ)濾波模型與PI控制模型：
[0118] 假設(shè)經(jīng)過校正后得到的姿態(tài)角度為Θ，則在s域上有如下等式成立
[0119]
(8：):
[0120] 其中，
域的低通濾波器，
為s域的高通濾波器，且有A (s)+B(s) = 1，k為角度補(bǔ)償參數(shù)，0rrf為解算出來的姿態(tài)角，L表示拉普拉斯變換， t表示測(cè)量 的起始時(shí)刻，At表示測(cè)量間隔，wm為陀螺儀測(cè)量值;式(8)的時(shí)域表達(dá)式為：
[0121]
(9)
[0122] 其中，θ^-θ為通過磁力計(jì)和加速度計(jì)計(jì)算得到的姿態(tài)角度的誤差量，用其校正陀 螺儀的角速度輸出，校正后的姿態(tài)角是需要利用ΡΙ調(diào)節(jié)器控制權(quán)重Κ的大?。?br>[0123] 互補(bǔ)濾波不需要建立精確的數(shù)學(xué)模型，只需要通過調(diào)整Κ的大小來獲得合適的截 止頻率：當(dāng)Κ較大時(shí)，截止頻率f較高，對(duì)低頻和幅值較大的加速度信息和磁力計(jì)信息濾除效 果差，互補(bǔ)濾波的測(cè)量結(jié)果主要依賴于加速度信息和磁力計(jì)信息；當(dāng)K較小時(shí)，截止頻率f較 低，互補(bǔ)濾波的測(cè)量結(jié)果主要依賴陀螺儀的角速度信息。
[0124] 在控制理論中存在大量的控制器，常見的有I積分控制器、P比例控制器以及復(fù)合 形式PI比例積分控制器等。
[0125] PI控制器的模型如下：
[0126]
(10)
[0127] 其中，e(t)為第t時(shí)刻系統(tǒng)的誤差，Uci(t)為第t時(shí)刻PI調(diào)節(jié)器的輸出，通過調(diào)節(jié)Ti和 I兩個(gè)參數(shù)即能獲得較好的誤差估計(jì)量。
[0128] (b)誤差矢量的建立
[0129] 對(duì)于陀螺誤差的校正，誤差量的計(jì)算是一個(gè)難點(diǎn)，誤差量的計(jì)算往往是基于梯度 下降法計(jì)算的，但是梯度下降法復(fù)雜并且計(jì)算量大，這里運(yùn)用向量叉乘來計(jì)算誤差矢量。
[0130] 首先用四元數(shù)旋轉(zhuǎn)矩陣估計(jì)當(dāng)前的加速度矢量if以及磁場(chǎng)強(qiáng)度矢量/T，具體公 式如下：
[0131] (11)
[0132] (丨、
[0133] 式中，礦為扭曲磁場(chǎng)校正后的值，f為本地加速度矢量，1?為四元數(shù)旋轉(zhuǎn)矩陣；
[0134] MEMS傳感器模塊輸出的加速度矢量以及磁場(chǎng)強(qiáng)度矢量量測(cè)值分別為士 .和,兩 者矢量之間的叉乘公式如下所示：
[0135] (13)
[0136] (14)
[0137] 理論情況下，Δι^ΡΔθ,γ值應(yīng)恒為〇, Δι^ΡΔθ,γ分別為磁場(chǎng)矢量與加速度矢量誤 差;但由于測(cè)量、數(shù)值截?cái)唷⒆鴺?biāo)對(duì)準(zhǔn)、非正交化等誤差的存在，此式不總為零;將上述兩種 向量進(jìn)行叉乘，向量積的長(zhǎng)度為兩向量夾角的正弦，并不完全與兩組向量的誤差角度成正 比但在誤差較小時(shí)，誤差角度與其正弦近似相等，因此
[0138] ( |Μ
[0139] (1.6)
[0140] 在這里i和4分別為加速度與磁場(chǎng)強(qiáng)度誤差矢量，假設(shè)沒有任何誤差存在，那么 估計(jì)旋轉(zhuǎn)該與量測(cè)向量之間重合即：
[0141] U7)
[0142] 存在誤差情況下，叉乘結(jié)果不等于零，由于誤差數(shù)量級(jí)很小，那么
[0143] (IH；
[0144] (19)
[0145] 空間誤差角度Θ投影到三個(gè)軸上形成誤差矢量，所以叉乘的結(jié)果可以去校正陀螺 所產(chǎn)生的漂移，獲得的總誤差矢量5為：
[0146]
(20)
[0147] (c)PI控制與互補(bǔ)濾波的融合
[0148] 三軸加速度計(jì)和三軸磁力計(jì)計(jì)算得到空間誤差矢量，利用互補(bǔ)濾波原理能夠校正 后的誤差量，即將式(20)帶入(10)式可得：
[0149]
(21)
[0150] 其中k為測(cè)量的采樣序列，j為積分序列，結(jié)合PI控制原理，通過對(duì)比例控制器和積 分控制器的系數(shù)的調(diào)整，能夠獲得更好的誤差估計(jì)量，即將式(21)帶入式(9)并轉(zhuǎn)化得：
[0151]
(22)
[0152] 在式(22)中令:twoKp = KKi，twoKi =KTi，#為校正后的陀螺儀數(shù)據(jù)，得到校正后輸 出的結(jié)果：
[0153]
(23)
[0154] 其中，twoKp表示比例積分，twoKi表示控制積分。
[0155]四、利用聯(lián)邦卡爾曼濾波數(shù)據(jù)融合算法對(duì)北斗與MEMS緊耦合系統(tǒng)的位置和速度誤 差進(jìn)行最優(yōu)估計(jì)，采用PI控制以及互補(bǔ)濾波算法對(duì)MEMS傳感器模塊所測(cè)量的三軸陀螺儀進(jìn) 行校正，具體過程如下：
[0156] (a)將MEMS傳感器模塊的九軸傳感器數(shù)據(jù)采用第一級(jí)卡爾曼濾波器，解算出行人 航向角信息和速度信息；
[0157] (b)將第一級(jí)卡爾曼濾波器輸出的行人位置、速度信息與北斗接收機(jī)模塊輸出的 偽距、偽距率原始觀測(cè)量，采用第二級(jí)卡爾曼濾波器解算出位置誤差和速度誤差；
[0158] (c)將第二級(jí)卡爾曼濾波器輸出的位置誤差和速度誤差反饋至PDR解算模塊，從而 對(duì)位置和速度信息進(jìn)行相應(yīng)的校正，得到最優(yōu)的定位結(jié)果，即是用戶位置信息；
[0159] 第一級(jí)卡爾曼濾波器和第二級(jí)卡爾曼濾波器組成所述步驟2的聯(lián)邦卡爾曼濾波 器。
[0160]五、根據(jù)實(shí)時(shí)接收到的北斗衛(wèi)星數(shù)據(jù)和MEMS傳感器數(shù)據(jù)，利用自適應(yīng)調(diào)整算法動(dòng) 態(tài)調(diào)整聯(lián)邦卡爾曼濾波算法中參數(shù)，能夠在不同環(huán)境下進(jìn)行軟切換，即是軟件上完成不同 環(huán)境下的參數(shù)調(diào)整，實(shí)現(xiàn)定位的無縫銜接，同時(shí)該系統(tǒng)利用北斗系統(tǒng)的秒脈沖信號(hào)實(shí)現(xiàn)北 斗衛(wèi)星模塊和MEMS傳感器模塊的同步，滿足導(dǎo)航同步需求。
[0161]自適應(yīng)調(diào)整算法通過聯(lián)邦卡爾曼濾波算法中過程噪聲協(xié)方差矩陣R來調(diào)整兩個(gè)定 位系統(tǒng)之間的權(quán)重，以實(shí)現(xiàn)定位系統(tǒng)之間的軟切換，從而滿足不同環(huán)境下的連續(xù)精確定位。 當(dāng)定位目標(biāo)所處環(huán)境發(fā)生變化(比如:從室內(nèi)至室外，或從室外開闊環(huán)境下至室外建筑物遮 蔽環(huán)境下），接收到的北斗信號(hào)會(huì)出現(xiàn)較大的波動(dòng)，若此時(shí)定位目標(biāo)從室外進(jìn)入室內(nèi)時(shí)，或 定位目標(biāo)從室外開闊環(huán)境下至室外建筑物遮蔽環(huán)境下，北斗信號(hào)會(huì)逐漸變?nèi)?，甚至無法接 收，那么此時(shí)無法利用北斗信息進(jìn)行數(shù)據(jù)融合，此時(shí)只能短時(shí)間利用MEMS解算信息進(jìn)行定 位。為了實(shí)現(xiàn)定位目標(biāo)的連續(xù)定位，可以通過過程噪聲協(xié)方差矩陣R來調(diào)整定位系統(tǒng)之間的 權(quán)重，特別是當(dāng)定位目標(biāo)完全沒有接收到衛(wèi)星信號(hào)時(shí)，定位模式會(huì)自動(dòng)被切換到MEMS傳感 器獨(dú)立定位，從而實(shí)現(xiàn)了定位的室內(nèi)外無縫銜接。
【主權(quán)項(xiàng)】
1. 一種基于航跡推算的低成本北斗與MEMS緊禪合定位系統(tǒng)，其特征在于：包括北斗接 收機(jī)模塊(2)、MEMS傳感器模塊（1 )、北斗與MEMS融合算法模塊(4)和ARM-Linux嵌入式系統(tǒng) 模塊(3)，所述ARM-Linux嵌入式系統(tǒng)模塊(3)分別與北斗接收機(jī)模塊(2)、MEMS傳感器模塊 (1)、北斗與MEMS融合算法模塊(4)連接； 北斗接收機(jī)模塊用于對(duì)衛(wèi)星信號(hào)的捕獲與跟蹤，并輸出偽距、偽距率和衛(wèi)星星歷信息； MEMS傳感器模塊包括處理器，W及分別與處理器連接的S軸加速度計(jì)、S軸巧螺儀和 =軸磁力計(jì)，處理器根據(jù)=軸加速度計(jì)、=軸巧螺儀和=軸磁力計(jì)所輸出的九軸傳感器信 息，估算出行人的航向角和速度信息，所述速度信息即為單位時(shí)間內(nèi)的步長(zhǎng)信息； 北斗與MEMS融合算法模塊將北斗接收機(jī)模塊所輸出的偽距、偽距率，W及MEMS傳感器 模塊所輸出的航向角、步長(zhǎng)信息，采用聯(lián)邦卡爾曼濾波數(shù)據(jù)融合算法，輸出最優(yōu)定位結(jié)果， 即是用戶位置信息； ARM-Linux嵌入式系統(tǒng)模塊用于實(shí)現(xiàn)北斗接收機(jī)模塊與MEMS傳感器模塊之間的數(shù)據(jù)同 步、數(shù)據(jù)處理、定位算法的實(shí)施W及定位結(jié)果的顯示。2. 根據(jù)權(quán)利要求1所述的基于航跡推算的低成本北斗與MEMS緊禪合定位系統(tǒng)，其特征 在于:所述ARM-Linux嵌入式系統(tǒng)模塊利用嵌入式外部中斷來檢測(cè)北斗接收機(jī)模塊