基于航跡推算的低成本北斗與mems緊耦合定位系統(tǒng)及方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及信號(hào)處理技術(shù)、數(shù)據(jù)融合技術(shù)、無(wú)線(xiàn)定位和傳感器定位技術(shù)領(lǐng)域，具體 涉及一種基于航跡推算的低成本北斗與MEMS緊耦合定位系統(tǒng)及方法。
【背景技術(shù)】
[0002] 隨著可移動(dòng)設(shè)備得到越來(lái)越廣泛的應(yīng)用以及無(wú)線(xiàn)網(wǎng)絡(luò)的普及，用戶(hù)對(duì)信息即時(shí)性 和就地性的需求越來(lái)越強(qiáng)烈，因此基于位置的服務(wù)也備受關(guān)注。目前較為流行的無(wú)線(xiàn)定位 系統(tǒng)有全球定位系統(tǒng)（GPS)、蜂窩定位系統(tǒng)、藍(lán)牙定位系統(tǒng)、射頻識(shí)別（RFID)定位系統(tǒng)、 ZigBee定位系統(tǒng)以及WLAN定位系統(tǒng)。其中GPS定位系統(tǒng)目前應(yīng)用最為廣泛，室外定位精度較 高，然而在障礙物較多的遮蔽或室內(nèi)環(huán)境下，如高樓林立的城市街道或室內(nèi)停車(chē)場(chǎng)，衛(wèi)星信 號(hào)會(huì)急劇減弱，導(dǎo)致GPS定位系統(tǒng)難以工作;蜂窩無(wú)線(xiàn)定位系統(tǒng)的定位精度較低，室外定位 誤差通常大于50米，對(duì)于定位精度要求較高的室內(nèi)場(chǎng)景則無(wú)法適用；藍(lán)牙、RFID和ZigBee等 定位技術(shù)一般較為適用于近距離定位環(huán)境。因此，上述定位技術(shù)不具有較好的普適性，而需 要尋找一種新的定位技術(shù)來(lái)代替或彌補(bǔ)各自的不足。同時(shí)，無(wú)線(xiàn)局域網(wǎng)的不斷普及給了 WLAN定位技術(shù)很大的發(fā)展機(jī)會(huì)。
[0003] 在WLAN定位系統(tǒng)中，位置指紋定位方法的精度較高且不需要添加額外的設(shè)備，從 而得到了較為廣泛的應(yīng)用?；谖恢弥讣y的定位方法主要分為兩個(gè)階段:離線(xiàn)階段和在線(xiàn) 階段。離線(xiàn)階段，在目標(biāo)區(qū)域內(nèi)選擇合適的參考點(diǎn)，并在參考點(diǎn)處測(cè)量來(lái)自每個(gè)AP的信號(hào)強(qiáng) 度值，建立位置指紋數(shù)據(jù)庫(kù)。在線(xiàn)階段，通過(guò)利用定位算法對(duì)接收端實(shí)時(shí)測(cè)量得到的信號(hào)值 與位置指紋數(shù)據(jù)庫(kù)中已保存的信號(hào)數(shù)據(jù)進(jìn)行匹配，來(lái)估計(jì)終端位置。
[0004] 由于室內(nèi)環(huán)境的復(fù)雜多變，因此位置指紋定位方法的精度性能受多種因素的影 響。通過(guò)分析對(duì)比影響位置指紋定位方法精度性能的因素，AP擺放位置對(duì)于定位精度有較 大影響。在現(xiàn)有方法中，為了優(yōu)化AP擺放位置，其主要關(guān)注的有信號(hào)覆蓋范圍，服務(wù)質(zhì)量，網(wǎng) 絡(luò)吞吐量，信號(hào)傳輸速率以及覆蓋范圍內(nèi)的信號(hào)差異性等因素。針對(duì)提高信號(hào)差異性這一 因素的各項(xiàng)研究中，現(xiàn)有方法都集中在使用某種搜索算法，去搜索一個(gè)使得與信號(hào)差異性 相關(guān)的目標(biāo)函數(shù)達(dá)到最大或最小值的組合解，這類(lèi)方法的主要缺陷是對(duì)搜索算法的依賴(lài)程 度較高且時(shí)間開(kāi)銷(xiāo)較大。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明針對(duì)現(xiàn)有室外定位技術(shù)存在的缺陷和不足，利用北斗衛(wèi)星信息和MEMS傳感 器信息，提出一種基于航跡推算的北斗與MEMS(Micr〇-Electr〇-Mechanical Systems，即微 電機(jī)系統(tǒng))融合定位系統(tǒng)，實(shí)現(xiàn)從室外進(jìn)入室內(nèi)時(shí)，或定位目標(biāo)從室外開(kāi)闊環(huán)境下至室外建 筑物遮蔽環(huán)境下的連續(xù)定位。
[0006] 本發(fā)明所述的一種基于航跡推算的低成本北斗與MEMS緊耦合定位系統(tǒng)，包括北斗 接收機(jī)模塊、MEMS傳感器模塊、北斗與MEMS融合算法模塊和ARM-Linux嵌入式系統(tǒng)模塊，所 述ARM-Linux嵌入式系統(tǒng)模塊分別與北斗接收機(jī)模塊、MEMS傳感器模塊、北斗與MEMS融合算 法模塊連接；
[0007] 北斗接收機(jī)模塊用于對(duì)衛(wèi)星信號(hào)的捕獲與跟蹤，并輸出偽距、偽距率和衛(wèi)星星歷 信息；
[0008] MEMS傳感器模塊包括處理器，以及分別與處理器連接的三軸加速度計(jì)、三軸陀螺 儀和三軸磁力計(jì)，處理器根據(jù)三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)所輸出的九軸傳感 器信息，估算出行人的航向角和速度信息，所述速度信息即為單位時(shí)間內(nèi)的步長(zhǎng)信息；
[0009] 北斗與MEMS融合算法模塊將北斗接收機(jī)模塊所輸出的偽距、偽距率，以及MEMS傳 感器模塊所輸出的航向角、步長(zhǎng)信息，采用聯(lián)邦卡爾曼濾波數(shù)據(jù)融合算法，輸出最優(yōu)定位結(jié) 果，即是用戶(hù)位置信息；
[0010] ARM-Linux嵌入式系統(tǒng)模塊用于實(shí)現(xiàn)北斗接收機(jī)模塊與MEMS傳感器模塊之間的數(shù) 據(jù)同步、數(shù)據(jù)處理、定位算法的實(shí)施以及定位結(jié)果的顯示。
[0011]所述ARM-Linux嵌入式系統(tǒng)模塊利用嵌入式外部中斷來(lái)檢測(cè)北斗接收機(jī)模塊的秒 脈沖信號(hào)是否到來(lái)，當(dāng)檢測(cè)到有秒脈沖信號(hào)到來(lái)時(shí)，則開(kāi)啟北斗接收機(jī)串口數(shù)據(jù)通道，同時(shí) 鎖定MEMS傳感器串口數(shù)據(jù)通道，當(dāng)北斗接收機(jī)模塊的數(shù)據(jù)寫(xiě)入完畢，將北斗接收機(jī)串口數(shù) 據(jù)通道切換至MEMS傳感器串口數(shù)據(jù)，至到檢測(cè)到下一個(gè)秒脈沖信號(hào)的到來(lái)。
[0012] 本發(fā)明所述的一種基于航跡推算的低成本北斗與MEMS緊耦合定位方法，采用本發(fā) 明所述的基于航跡推算的低成本北斗與MEMS緊耦合定位系統(tǒng)，包括以下步驟：
[0013] 步驟1、從北斗接收機(jī)模塊中提取出偽距、偽距率、衛(wèi)星星歷信息，利用修正算法對(duì) 偽距和偽距率信息進(jìn)行修正處理；同時(shí)利用初始位置和速度解算算法從北斗接收機(jī)模塊中 得到北斗與MEMS緊耦合系統(tǒng)的初始位置和速度信息，并將該初始位置和速度信息用于PDR 算法的初始化；
[0014] 步驟2、利用MEMS傳感器模塊所測(cè)量的三軸加速度數(shù)據(jù)、三軸磁力計(jì)數(shù)據(jù)，通過(guò)PI 控制以及互補(bǔ)濾波算法對(duì)三軸陀螺儀數(shù)據(jù)進(jìn)行校正，然后將校正后的九軸傳感器數(shù)據(jù)送入 基于四元數(shù)的卡爾曼姿態(tài)更新算法進(jìn)行姿態(tài)解算，估算出行人的航向角信息；
[0015] 利用步態(tài)以及步頻檢測(cè)算法對(duì)三軸加速度計(jì)數(shù)據(jù)進(jìn)行分析，從而估算出行人的步 長(zhǎng)信息，再利用每秒的步長(zhǎng)值估算出行人的速度；
[0016] 將估算出的行人的航向角和速度，結(jié)合北斗接收機(jī)模塊所輸出的衛(wèi)星星歷信息， 利用roR算法估算出基于行人的偽距和偽距率；
[0017] 步驟3、將所述步驟2計(jì)算出的基于行人的偽距、偽距率分別與所述步驟1修正后的 基于北斗接收機(jī)模塊的偽距、偽距率--對(duì)應(yīng)作差，得到偽距差、偽距率差，并將該偽距差、 偽距率差作為聯(lián)邦卡爾曼濾波算法的量測(cè)輸入值，同時(shí)將東向、北向位置誤差、垂直誤差， 東向、北向速度誤差、高度速度誤差，等效時(shí)鐘誤差相對(duì)應(yīng)的距離，等效時(shí)鐘誤差相對(duì)應(yīng)的 距離率，航向角偏差誤差量作為聯(lián)邦卡爾曼濾波算法的狀態(tài)變量，利用聯(lián)邦卡爾曼濾波數(shù) 據(jù)融合算法對(duì)北斗與MEMS緊耦合系統(tǒng)的位置和速度誤差進(jìn)行最優(yōu)估計(jì)；
[0018] 步驟4、將位置和速度誤差反饋到PDR解算模塊，從而對(duì)位置和速度信息進(jìn)行相應(yīng) 的校正，得到最優(yōu)的定位結(jié)果，即是用戶(hù)位置信息。
[0019] 所述狀態(tài)變量如下：
[0020] Χ=[δΕ δΝ 5h 5Ve δνΝ 5Vh 5tu 5tru]T (1)
[0021] 其中變量分別為：東向北向位置誤差δΕ、δΝ，垂直誤差δ?ι，東向和北向速度誤差δ νΕ、δνΝ，垂直速度誤差SVh，等效時(shí)鐘誤差相應(yīng)的距離Stu，等效時(shí)鐘誤差相應(yīng)的距離率St ru， 狀態(tài)方程為：
[0022]
(2)
[0023] 其中ω k為相應(yīng)的噪聲向量，ω Ε、ω n、ω h、_氣、％、ω u、ω ru分別為狀態(tài)量 的噪聲值，系統(tǒng)的觀(guān)測(cè)方程為：
[0024] (3)
[0025]
[0026] (4)
[0027] (5)
[0028]其中Pn為MEMS信息與第i顆衛(wèi)星信息計(jì)算得到的偽距，pBl*BD接收機(jī)測(cè)得第i顆 衛(wèi)星的偽距，e^(j = l，2,3)為接收機(jī)相對(duì)于第i顆衛(wèi)星的三維方向余弦，δ為相應(yīng)位置速度 誤差，V為量測(cè)噪聲矢量，Vp1、 Vp2、Vp3Vp4分別為量測(cè)偽距噪聲、％、％、々、％分別為量測(cè) 偽距率噪聲。
[0029]所述PDR算法主要是根據(jù)每個(gè)時(shí)刻由MEMS模塊解算出的航向角信息和速度信息， 遞推出每個(gè)時(shí)刻的行人位置信息，具體步驟如下：
[0030] 利用MEMS傳感器模塊所檢測(cè)的三軸加速度計(jì)、三軸磁力計(jì)和三軸陀螺儀數(shù)據(jù)，估 算出行人的步長(zhǎng)信息cU和航向角信息Θ,，結(jié)合從北斗接收機(jī)模塊直接獲取到的行人位置信 息，即為初始位置Ρ〇(Νο，Εο)，則可以遞推出第η時(shí)刻的行人位置?"(化而），可由下式確定：
[0031] (6)
[0032]
[0033] 其中，和di為第i次位移方向和位移距離，Nk為第k時(shí)刻的北向位置值，Ek為第k時(shí) 刻的東向位置值;將濾波估計(jì)誤差結(jié)果對(duì)PDR解算出來(lái)的位置信息進(jìn)行相應(yīng)坐標(biāo)軸上的校 正。
[0034]所述步長(zhǎng)的估算方法，定義stride (m)為步長(zhǎng)公式，則根據(jù)三軸加速度計(jì)數(shù)據(jù)計(jì)算 得到的步萇彳古彳彳太加下·
[0035]
(7)
[0036]其中：ax，ay，az分別為三軸加速度的X軸、y軸、z軸輸出，a maJPamir^v別為三軸加速 度計(jì)輸出加速度模值的最大值max和最小值min，即是從抬腳時(shí)刻到落腳時(shí)刻之間的最大值 和最小值;tup和tdmm對(duì)應(yīng)抬腳up、落腳down時(shí)刻。
[0037]利用MEMS傳感器模塊所測(cè)量的三軸加速度數(shù)據(jù)、三軸磁力計(jì)數(shù)據(jù)，通過(guò)PI控制以 及互補(bǔ)濾波算法對(duì)三軸陀螺儀數(shù)據(jù)進(jìn)行校正，具體校正過(guò)程如下：
[0038] (a)互補(bǔ)濾波模型與PI控制模型：
[0039]假設(shè)經(jīng)過(guò)校正后得到的姿態(tài)角度為Θ，則在s域上有如下等式成立
[0040]
.(8)
[0041] 其中，域的低通濾波器，與4 = ^7為s域的高通濾波器，且有A 、丁k s+k (s)+B(s) = 1，k為角度補(bǔ)償參數(shù)，0rrf為解算出來(lái)的姿態(tài)角，L表示拉普拉斯變換， t表示測(cè)量 的起始時(shí)刻，At表示測(cè)量間隔，wm為陀螺儀測(cè)量值;式(8)的時(shí)域表達(dá)式為：
[0042]
(9)
[0043]其中，0ref-0為通過(guò)磁力計(jì)和加速度計(jì)計(jì)算得到的姿態(tài)角度的誤差量，用其校正陀 螺儀的角速度輸出，校正后的姿態(tài)角是需要利用PI調(diào)節(jié)器控制權(quán)重K的大小；
[0044] PI控制器的模型如下：
[0045] u〇(t) = Kie(t)+TiJe(t)dt (10)
[0046] 其中，e(t)為第t時(shí)刻系統(tǒng)的誤差，Uci(t)為第t時(shí)刻PI調(diào)節(jié)器的輸出，通過(guò)調(diào)節(jié)Ti和 I兩個(gè)參數(shù)即能獲得較好的誤差估計(jì)量；
[0047] (