一種基于mems的ins/gps組合的室內(nèi)外個(gè)人導(dǎo)航算法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及基于MEMS的導(dǎo)航與定位技術(shù)領(lǐng)域，特別是在室內(nèi)外的個(gè)人導(dǎo)航領(lǐng)域 具有廣泛的應(yīng)用。
【背景技術(shù)】
[0002] 個(gè)人導(dǎo)航系統(tǒng)（PersonalNavigationSystem，PNS)，又稱為步行者導(dǎo)航系統(tǒng)，可 以實(shí)時(shí)確定行人行進(jìn)方向、速度、位置等信息，是導(dǎo)航定位領(lǐng)域中的一個(gè)重要的新興分支， 主要用于跟蹤定位徒步行走人員的實(shí)時(shí)位置。MEMS器件具有體積小、功耗低、質(zhì)量輕、靈敏 度高、成本低的優(yōu)點(diǎn)，基于MEMS的個(gè)人導(dǎo)航系統(tǒng)的研宄已經(jīng)成為一大研宄熱點(diǎn)。在單兵作 戰(zhàn)、消防人員的定位以及山區(qū)叢林探險(xiǎn)者等場(chǎng)景都具有廣泛的應(yīng)用前景。
[0003] 在空曠區(qū)，全球定位系統(tǒng)（GPS)的精確度在幾米之內(nèi)，而且定位誤差不隨時(shí)間而 積累，但在建筑物密集區(qū)，它甚至經(jīng)常無(wú)法正確定位城市街區(qū)。問題在于多路徑干擾，多路 徑是GPS在城市和室內(nèi)的主要干擾源，其他干擾都不會(huì)造成如此嚴(yán)重的誤差，這是所有基 于無(wú)線電的定位系統(tǒng)的災(zāi)難。所以GPS很多時(shí)候受到環(huán)境的限制，而無(wú)法進(jìn)行導(dǎo)航與定位。
[0004] 慣性導(dǎo)航系統(tǒng)不依賴任何外界系統(tǒng)的支持就能獨(dú)立自主地進(jìn)行導(dǎo)航，可以連續(xù)地 提供包括姿態(tài)基準(zhǔn)在內(nèi)的全部導(dǎo)航和制導(dǎo)參數(shù)，對(duì)準(zhǔn)后短期時(shí)間內(nèi)具有良好的精度和穩(wěn)定 性。但是INS結(jié)構(gòu)較復(fù)雜，導(dǎo)航誤差隨時(shí)間積累而增大，對(duì)準(zhǔn)時(shí)間比較長(zhǎng)，因此，不能滿足遠(yuǎn) 距離或長(zhǎng)時(shí)間航行以及高精度導(dǎo)航或制導(dǎo)的要求。

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

[0005] 為了提高導(dǎo)航定位精度，出現(xiàn)了多種組合導(dǎo)航的方式，即把各具特點(diǎn)的不同類型 的導(dǎo)航系統(tǒng)匹配組合，使之相互取長(zhǎng)補(bǔ)短，從而形成一種更為優(yōu)良的新型導(dǎo)航系統(tǒng)一一組 合導(dǎo)航系統(tǒng)，其中GPS/INS組合導(dǎo)航系統(tǒng)最為先進(jìn)且應(yīng)用最為廣泛。而本發(fā)明主要是基于 個(gè)人的GPS/INS組合導(dǎo)航系統(tǒng)。
[0006] 本發(fā)明采用的技術(shù)方案為一種基于MEMS的INS/GPS組合的室內(nèi)外個(gè)人導(dǎo)航算法， 該方法的實(shí)現(xiàn)步驟如下，
[0007] (1)多步態(tài)零速檢測(cè)算法
[0008] 在捷聯(lián)解算過(guò)程中，如果不對(duì)誤差進(jìn)行及時(shí)的修正，位置誤差會(huì)以時(shí)間的三次方 迅速發(fā)散，最終使導(dǎo)航功能喪失。人在正常行走過(guò)程中每走一步腳部與地面接觸時(shí)都會(huì)有 一個(gè)相對(duì)靜止的瞬間，也稱作是零速間隔。準(zhǔn)確得檢測(cè)零速間隔，并在零速間隔對(duì)誤差實(shí)時(shí) 修正可以有效抑制誤差的累積效應(yīng)。本方法采用加速度方差、陀螺比力模值相結(jié)合的自調(diào) 節(jié)閾值的零速檢測(cè)算法。通過(guò)一個(gè)基于平均加速度的滑動(dòng)時(shí)間窗口，來(lái)判斷行人運(yùn)動(dòng)的步 態(tài)模式，根據(jù)行人的狀態(tài)兩種檢測(cè)方法自調(diào)節(jié)選用合適的閾值。檢測(cè)過(guò)程如下：
[0009] 1)判斷運(yùn)動(dòng)模式：
[0010] 此時(shí)MTI的X軸與人體前進(jìn)方向相同，計(jì)算X軸的基于時(shí)間窗口的平均加速度來(lái) (JN 104819716 A ^ TJ 2/6貝 判斷運(yùn)動(dòng)模式，k時(shí)刻的X軸平均加速度為：
【主權(quán)項(xiàng)】
1. 一種基于MEMS的INS/GPS組合的室內(nèi)外個(gè)人導(dǎo)航算法，其特征在于：該方法的實(shí)現(xiàn) 步驟如下， (1) 多步態(tài)零速檢測(cè)算法 在捷聯(lián)解算過(guò)程中，如果不對(duì)誤差進(jìn)行及時(shí)的修正，位置誤差會(huì)以時(shí)間的三次方迅速 發(fā)散，最終使導(dǎo)航功能喪失；人在正常行走過(guò)程中每走一步腳部與地面接觸時(shí)都會(huì)有一個(gè) 相對(duì)靜止的瞬間，也稱作是零速間隔；準(zhǔn)確得檢測(cè)零速間隔，并在零速間隔對(duì)誤差實(shí)時(shí)修正 可以有效抑制誤差的累積效應(yīng)；本方法采用加速度方差、陀螺比力模值相結(jié)合的自調(diào)節(jié)閾 值的零速檢測(cè)算法；通過(guò)一個(gè)基于平均加速度的滑動(dòng)時(shí)間窗口，來(lái)判斷行人運(yùn)動(dòng)的步態(tài)模 式，根據(jù)行人的狀態(tài)兩種檢測(cè)方法自調(diào)節(jié)選用合適的閾值；檢測(cè)過(guò)程如下： 1) 判斷運(yùn)動(dòng)模式： 此時(shí)MTI的X軸與人體前進(jìn)方向相同，計(jì)算X軸的基于時(shí)間窗口的平均加速度來(lái)判斷
走狀態(tài)的判斷依據(jù)，根據(jù)行走的狀態(tài)加速度方差的閾值UhlJ和陀螺模值平方和的閾值 (thj分別選擇不同的值； 2) 在k時(shí)刻加速度方差和陀螺模值平方和的大?。?br>〇 a加速度方差，《 ,為三軸的陀螺平方和； 3) 兩種檢測(cè)方法的融合： 當(dāng)加速度方差〇 3小于th。3時(shí)，加速度的零速檢測(cè)波形為低電平；同理當(dāng)陀螺模值的 平方和小于也"時(shí)，陀螺的零速檢測(cè)波形也為低電平；然后將兩種零速檢測(cè)結(jié)果進(jìn)行與 計(jì)算；在兩種檢測(cè)方法相融合時(shí)常常會(huì)帶來(lái)一些高頻噪聲，通過(guò)改進(jìn)的卡爾曼濾波算法剔 除高頻噪聲后進(jìn)行零速修正； (2) 融合室內(nèi)地圖的粒子濾波 粒子濾波的核心思想是先在狀態(tài)空間中產(chǎn)生一組隨機(jī)樣本粒子，然后在測(cè)量的基礎(chǔ) 上，通過(guò)調(diào)節(jié)權(quán)值的大小和樣本的位置從而獲得服從實(shí)際分布的樣本，并以樣本的均值作 為狀態(tài)估計(jì)值；這里在室內(nèi)平面地圖已知的情況下，采用融合室內(nèi)地圖的粒子濾波算法對(duì) 導(dǎo)航軌跡進(jìn)行修正，從而得到最優(yōu)的解算結(jié)果； 魯粒子初始化 當(dāng)k = O時(shí)，根據(jù)狀態(tài)先驗(yàn)分布p (Xtl)建立初始狀態(tài)粒子集jxf, i，其中每一個(gè)樣
魯f目息提取 根據(jù)零速修正算法，得到人行走過(guò)程中的起始時(shí)刻和終止時(shí)刻，經(jīng)過(guò)卡爾曼濾波算法 進(jìn)行零速修正以后，得到慣性導(dǎo)航解算的位置rk和航向角k信息，然后根據(jù)運(yùn)動(dòng)模型提取 出每一步的步長(zhǎng)I k和航向角變化量S it k，即：
?預(yù)測(cè) 根據(jù)系統(tǒng)狀態(tài)方程（4)，由k-1時(shí)刻的位置和航向角的變化信息得到k時(shí)刻的狀態(tài)值 Xf;
魯權(quán)值計(jì)算 根據(jù)觀測(cè)值與預(yù)測(cè)值的關(guān)系計(jì)算每一個(gè)粒子的權(quán)重即：
結(jié)合歐氏相似度與余弦相似度原理（5)式中可按下式表示：
其中，L、0分別為k時(shí)刻第i個(gè)粒子的位置、航向角與慣性導(dǎo)航解算的位置、航向角的 差值； ?融合室內(nèi)地圖 由于人的行走軌跡不可能穿越墻壁，為航位推算的正確性提供了客觀依據(jù)；這里通過(guò) 結(jié)合地圖平面信息，即墻壁信息，對(duì)每一個(gè)粒子k時(shí)刻的預(yù)測(cè)值吣"與前一時(shí)刻的預(yù)測(cè)值 X=的連接路徑進(jìn)行判定，如果穿越墻壁，權(quán)重則置為零，從而對(duì)無(wú)效粒子進(jìn)行剔除； 魯權(quán)值歸一化 將優(yōu)化后的粒子權(quán)重進(jìn)行歸一化即：
?自適應(yīng)重采樣 融合室內(nèi)地圖的粒子濾波算法，使得穿越墻壁粒子權(quán)重此時(shí)將回溯到上一步 從新更新粒子，經(jīng)若干次嘗試后生成的新粒子仍然穿墻就將其標(biāo)記為無(wú)效粒子；當(dāng)所有粒 子全部穿越墻壁或絕大多數(shù)粒子穿越墻壁時(shí)嚴(yán)重加快了粒子的退化速度，導(dǎo)致權(quán)重歸一化 失敗，進(jìn)而導(dǎo)致粒子重采樣失去意義使推算出現(xiàn)嚴(yán)重偏差；為此針對(duì)大誤差數(shù)據(jù)采用了一 種自適應(yīng)的重采樣方案；設(shè)置經(jīng)過(guò)穿墻判別以后權(quán)重不為O的粒子為有效粒子，用N rff表示
定是否進(jìn)行重采樣；這樣就有效避免了有效粒子數(shù)過(guò)少即粒子嚴(yán)重退化時(shí)，重采樣而導(dǎo)致 的粒子貧化問題； (3)室外GPS/INS組合導(dǎo)航 本方法采用GPS/INS組合模式，其工作原理是：將GPS輸出的位置、速度信息和慣導(dǎo)輸 出的位置、速度信息進(jìn)行作差，經(jīng)過(guò)卡爾曼濾波器進(jìn)行濾波，估計(jì)出慣導(dǎo)系統(tǒng)的誤差，然后 對(duì)慣導(dǎo)系統(tǒng)進(jìn)行校正來(lái)提高導(dǎo)航的精度； 在INS/GPS松耦合組合導(dǎo)航誤差模型中，選取INS的姿態(tài)誤差、速度誤差、位置誤差以 及陀螺儀和加速度計(jì)的常值漂移誤差作為組合導(dǎo)航系統(tǒng)的狀態(tài)變量，其狀態(tài)變量為：
式中：外，％，％分別為東向、北向和天向的三個(gè)姿態(tài)誤差角；Sve，Svn， S Vu別為東 向、北向和天向速度誤差；S L, S A，6 h分別為煒度、經(jīng)度和高度誤差；ebx,eby,e bz分別 為三軸陀螺誤差；▽ ax，▽ ay，▽ az分別為加速度計(jì)誤差； 由于單純的使用慣性器件進(jìn)行個(gè)人定位，導(dǎo)航誤差隨著時(shí)間的增長(zhǎng)也會(huì)越來(lái)越大；為 了對(duì)航向累積誤差進(jìn)行校正，在室外有GPS信號(hào)環(huán)境下釆用融合GPS和航跡推算的方法進(jìn) 行組合定位導(dǎo)航修正航向角，提高個(gè)人定位導(dǎo)航的準(zhǔn)確性和穩(wěn)定； 本方法采用GPS測(cè)得的相鄰有效信息對(duì)于GPS測(cè)得位置的準(zhǔn)確性進(jìn)行判斷以此來(lái)決定 系統(tǒng)是否進(jìn)入組合模式；判斷方法在傳統(tǒng)的利用接受衛(wèi)星數(shù)量和精度估計(jì)因子的判別基礎(chǔ) 上，增加了一個(gè)判別識(shí)別窗口，只有當(dāng)側(cè)得的位置信息在窗口內(nèi)連續(xù)有效并且位置偏差小 于一定值時(shí)，系統(tǒng)才進(jìn)入真正的組合模式。
【專利摘要】一種基于MEMS的INS/GPS組合的室內(nèi)外個(gè)人導(dǎo)航算法，該方法的實(shí)現(xiàn)步驟如下，在INS/GPS松耦合組合導(dǎo)航誤差模型中，選取INS的姿態(tài)誤差、速度誤差、位置誤差以及陀螺儀和加速度計(jì)的常值漂移誤差作為組合導(dǎo)航系統(tǒng)的狀態(tài)變量，由于單純的使用慣性器件進(jìn)行個(gè)人定位，導(dǎo)航誤差隨著時(shí)間的增長(zhǎng)也會(huì)越來(lái)越大。本發(fā)明采用GPS測(cè)得的相鄰有效信息對(duì)于GPS測(cè)得位置的準(zhǔn)確性進(jìn)行判斷以此來(lái)決定系統(tǒng)是否進(jìn)入組合模式。判斷方法在傳統(tǒng)的利用接受衛(wèi)星數(shù)量和精度估計(jì)因子的判別基礎(chǔ)上，增加了一個(gè)判別識(shí)別窗口。通過(guò)融合室內(nèi)地圖的粒子濾波算法，通過(guò)判斷粒子推算結(jié)果是否符合客觀事實(shí)，來(lái)修正原始算法的航向誤差以此對(duì)行人軌跡進(jìn)行優(yōu)化，能夠比較好的提供長(zhǎng)時(shí)間高精度的室內(nèi)自主導(dǎo)航。
【IPC分類】G01S19-47, G01C21-16
【公開號(hào)】CN104819716
【申請(qǐng)?zhí)枴緾N201510191794
【發(fā)明人】王普, 潘凱, 任明榮, 張曉東, 劉艷紅
【申請(qǐng)人】北京工業(yè)大學(xué)
【公開日】2015年8月5日
【申請(qǐng)日】2015年4月21日