全姿態(tài)環(huán)境下自主室內(nèi)定位算法
【專利摘要】本專利公開(kāi)了一種實(shí)現(xiàn)全姿態(tài)環(huán)境下自主室內(nèi)定位算法，首先，利用便攜式手持終端里集成的低成本運(yùn)動(dòng)姿態(tài)測(cè)量模塊中的MEMS慣性傳感器實(shí)現(xiàn)手持姿態(tài)下自主室內(nèi)定位功能；然后，在手持姿態(tài)的基礎(chǔ)上，利用姿態(tài)切換檢測(cè)算法完成對(duì)不同姿態(tài)之間的切換的精確跟蹤，最后，在手持姿態(tài)定位算法和姿態(tài)切換檢測(cè)的基礎(chǔ)上，并結(jié)合不同姿態(tài)切換下的旋轉(zhuǎn)矩陣實(shí)現(xiàn)了基于運(yùn)動(dòng)姿態(tài)測(cè)量模塊中的MEMS慣性傳感器在不同姿態(tài)下的自主室內(nèi)定位功能。該算法的主要有以下四部分組成：對(duì)姿態(tài)切換過(guò)程精確的跟蹤、姿態(tài)切換后對(duì)姿態(tài)精確分類、姿態(tài)切換過(guò)程中姿態(tài)角精度優(yōu)化及各種姿態(tài)下自主定位算法實(shí)現(xiàn)與更新準(zhǔn)則。便攜式手持終端里集成的低成本運(yùn)動(dòng)姿態(tài)模塊作為本算法實(shí)現(xiàn)的硬件平臺(tái)。
【專利說(shuō)明】全姿態(tài)環(huán)境下自主室內(nèi)定位算法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及MEMS慣性傳感器導(dǎo)航領(lǐng)域，特別涉及姿態(tài)自由變換在保持自主慣性 導(dǎo)航精度領(lǐng)域中的應(yīng)用。

【背景技術(shù)】
[0002] 行人在利用便攜式手持終端進(jìn)行室內(nèi)定位時(shí)，長(zhǎng)時(shí)間保持在手持姿態(tài)的可能性不 大。在定位階段，可能會(huì)將設(shè)備放在耳邊，也可能將手機(jī)放在腰間的口袋里；將上述便攜式 手持終端用于戰(zhàn)場(chǎng)士兵在不固定室內(nèi)環(huán)境下的定位，如果定位方式只限制為手持，那么對(duì) 于戰(zhàn)場(chǎng)上的士兵來(lái)說(shuō)，不可能長(zhǎng)時(shí)間保持一種姿態(tài)不變?？紤]上述二種情況，單一的姿態(tài)定 位技術(shù)越來(lái)越不能滿足定位的需求，在這種情況下，基于全姿態(tài)環(huán)境下的自主室內(nèi)定位技 術(shù)就顯得越來(lái)越重要了。
[0003] 目前對(duì)于不同姿態(tài)下能實(shí)現(xiàn)室內(nèi)定位的技術(shù)且應(yīng)用較為廣泛的有藍(lán)牙 (Bluetooth)、WLAN、紅外線（Location Based Service, LBS)、超寬帶（Ultra-Wideband, UWB)、射頻識(shí)別（Radio Frequency Identification, RFID)等。這些定位技術(shù)由于需要事 先在定位區(qū)域進(jìn)行布點(diǎn)、采集參考點(diǎn)信號(hào)強(qiáng)度并建立離線信號(hào)強(qiáng)度庫(kù)來(lái)進(jìn)行對(duì)目標(biāo)進(jìn)行定 位。首先，由于無(wú)線信號(hào)在室內(nèi)環(huán)境下容易受墻壁、地板、空氣中固態(tài)懸浮物等各種干擾，從 而嚴(yán)重影響最終的定位精度；其次，這些定位方式不能實(shí)現(xiàn)自主條件下的室內(nèi)定位。
[0004] 基于MEMS慣性傳感器的全姿態(tài)環(huán)境下進(jìn)行自主室內(nèi)定位是目前研究的熱點(diǎn)。首 先，這種室內(nèi)定位方式不需要在定位區(qū)域進(jìn)行布點(diǎn)，極大減小了成本；其實(shí)，它能完全自主 的進(jìn)行定位，不依靠任何外界條件。這些優(yōu)勢(shì)確定了基于MEMS慣性傳感器的全姿態(tài)下自主 室內(nèi)定位將會(huì)在未來(lái)定位領(lǐng)域占有很大的市場(chǎng)。
[0005] 利用MEMS慣性傳感器則能實(shí)現(xiàn)在室內(nèi)環(huán)境下的自主定位。目前主要是利用數(shù)據(jù) 融合算法對(duì)MEMS陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)三個(gè)慣性傳感器的數(shù)據(jù)進(jìn)行融合，從而實(shí)現(xiàn)自 主室內(nèi)定位。其主要的數(shù)據(jù)融合算法有卡爾曼濾波（KF)、擴(kuò)展卡爾曼濾波（EKF)、無(wú)跡卡爾 曼濾波（UKF)以及粒子濾波。但是上述描述的定位技術(shù)只在MEMS慣性傳感器處于相對(duì)穩(wěn) 定的姿態(tài)下才能對(duì)目標(biāo)進(jìn)行定位，這極大的限制了 MEMS慣性導(dǎo)航的應(yīng)用領(lǐng)域。


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

[0006] 有鑒于此，本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種全姿態(tài)環(huán)境下自主室內(nèi)定位算 法；克服傳統(tǒng)MEMS慣性傳感器自主室內(nèi)定位方式由于姿態(tài)變換導(dǎo)致定位算法癱瘓等缺陷， 拓展了 MEMS慣性傳感器自主室內(nèi)定位應(yīng)用領(lǐng)域。
[0007] 本發(fā)明的目的之一是提出一種各種不同姿態(tài)下自主室內(nèi)定位算法；本發(fā)明的目的 之二是提出一種各種不同姿態(tài)切換時(shí)，自主室內(nèi)定位算法的實(shí)時(shí)更新。
[0008] 本發(fā)明的目的之一是通過(guò)以下技術(shù)方案來(lái)實(shí)現(xiàn)的： 本發(fā)明提供的各種不同姿態(tài)下自主室內(nèi)定位算法的硬件平臺(tái)是便攜式手持終端里集 成的MEMS慣性測(cè)量單元。姿態(tài)切換完成后，利用加速度計(jì)和磁強(qiáng)計(jì)來(lái)重新解算切換后姿態(tài) 的初始姿態(tài)角，并將其轉(zhuǎn)化成四元數(shù)，為后期濾波提供基礎(chǔ)；同時(shí)由于姿態(tài)切換前后，姿態(tài) 角存在一個(gè)差值，在姿態(tài)切換穩(wěn)定后，將差值進(jìn)行消除。
[0009] 本發(fā)明的目的之二是通過(guò)以下技術(shù)方案來(lái)實(shí)現(xiàn)的： 本發(fā)明提供的各種不同姿態(tài)切換時(shí)，自主室內(nèi)定位算法的實(shí)時(shí)更新的條件，包括三軸 加速度計(jì)數(shù)據(jù)模值、三軸加速度計(jì)單軸數(shù)據(jù)正負(fù)以及橫滾角正負(fù)變化情況； 所述三軸加速度計(jì)數(shù)據(jù)模值以及三軸加速度計(jì)單軸數(shù)據(jù)正負(fù)，用于判斷姿態(tài)切換類 型； 所述姿態(tài)切換類型包括 所述橫滾角正負(fù)用于判斷姿態(tài)切換的方向； 所述姿態(tài)切換方向包括姿態(tài)向左切換和姿態(tài)向右切換； 本發(fā)明的優(yōu)點(diǎn)在于：用戶在定位階段不受姿態(tài)的嚴(yán)格限制，在姿態(tài)種類的容許范圍內(nèi)， 用戶可以根據(jù)需求更換定位的姿態(tài)，對(duì)基于MEMS慣性傳感器實(shí)現(xiàn)的自主室內(nèi)定位起到極 大的推動(dòng)作用。

【專利附圖】

【附圖說(shuō)明】
[0010] 為了使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對(duì)本發(fā)明作進(jìn) 一步的詳細(xì)描述，其中： 圖1為本發(fā)明硬件框圖； 圖2為手持姿態(tài)下定位算法框圖； 圖3為姿態(tài)切換跟蹤算法框圖； 圖4為姿態(tài)切換后對(duì)姿態(tài)分類的算法框圖； 圖5為姿態(tài)切換過(guò)程中姿態(tài)角處理算法框圖； 圖6為姿態(tài)切換前后固定姿態(tài)角差算法框圖； 圖7為全姿態(tài)算法流程框圖。

【具體實(shí)施方式】
[0011] 以下將結(jié)合附圖，對(duì)本發(fā)明的優(yōu)選實(shí)施例進(jìn)行詳細(xì)的描述；應(yīng)當(dāng)理解，優(yōu)選實(shí)施例 僅為了說(shuō)明本發(fā)明，而不是為了限制本發(fā)明的保護(hù)范圍。
[0012] 實(shí)施例1 圖1為本發(fā)明硬件框圖；圖2為手持姿態(tài)下定位算法框圖；圖3為姿態(tài)切換跟蹤算法 框圖；圖4為姿態(tài)切換后對(duì)姿態(tài)分類的算法框圖，圖5為姿態(tài)切換過(guò)程中姿態(tài)角處理算法框 圖，圖6為姿態(tài)切換前后固定姿態(tài)角差算法框圖，圖7為全姿態(tài)算法流程框圖； 所述硬件框圖中主要含有三軸MEMS陀螺儀、三軸MEMS加速度計(jì)和三軸MEMS磁強(qiáng)計(jì)； 所述手持姿態(tài)下定位算法下的自主室內(nèi)定位，即在定位的全過(guò)程中定位姿態(tài)維持在手 持姿態(tài)下； 所述姿態(tài)切換跟蹤為利用加速度計(jì)數(shù)據(jù)和橫滾角信息實(shí)時(shí)檢測(cè)姿態(tài)是否在進(jìn)行切 換； 所述姿態(tài)切換后對(duì)姿態(tài)分類為利用加速度計(jì)三軸數(shù)據(jù)和橫滾角正負(fù)變化情況來(lái)對(duì)姿 態(tài)切換后的姿態(tài)進(jìn)行精確的分類，其切換的姿態(tài)可以分為向左切換姿態(tài)和向右切換姿態(tài)二 大類； 所述加速度三軸數(shù)據(jù)正負(fù)組合變化，包括X軸、Y軸和Z軸正負(fù)組合方式； 所述向左切換姿態(tài)為：手持右一耳邊左、手持右一腰間左、腰間右一耳邊左、腰間右一 手持左、耳邊右--手持左以及耳邊右一腰間左； 所述向右切換姿態(tài)為：手持左一耳邊右、手持左一腰間右、腰間左一耳邊右、腰間左一 手持右、耳邊左一手持右及耳邊左一腰間右； 所述姿態(tài)切換過(guò)程中姿態(tài)角處理為利用姿態(tài)切換前可靠的姿態(tài)角信息代替姿態(tài)切換 過(guò)程中的姿態(tài)角，其技術(shù)解決方案如下： 在無(wú)磁干擾的環(huán)境下，利用姿態(tài)切換前0. 2s的傳感器數(shù)據(jù)解算出的所有姿態(tài)角的平 均值來(lái)代替姿態(tài)切換過(guò)程中姿態(tài)角；如果這〇. 2s內(nèi)有磁干擾，那么就在這0. 2s的基礎(chǔ)上向 前取0. 2s傳感器數(shù)據(jù)，依次類推，一直找到?jīng)]有干擾的0. 2s的傳感器數(shù)據(jù)。
[0013] 所述附圖2中的初始姿態(tài)由磁強(qiáng)計(jì)和加速度計(jì)解算得來(lái)，再將初始姿態(tài)轉(zhuǎn)成初始 四元數(shù)，后期采用利用前一時(shí)刻四元數(shù)、陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)經(jīng)EKF (擴(kuò)展卡爾曼濾 波)濾波得出當(dāng)前時(shí)刻四元數(shù)，然后利用當(dāng)前時(shí)刻的四元數(shù)更新姿態(tài)角信息。但是姿態(tài)在切 換過(guò)程中由于陀螺儀和加速度計(jì)的數(shù)據(jù)都是不可信的，故在姿態(tài)切換過(guò)程中EKF濾波得出 的當(dāng)前四元數(shù)是不可靠的，從而導(dǎo)致姿態(tài)角也是不可靠的。如果在姿態(tài)切換后，姿態(tài)角的更 新依舊利用前一刻四元數(shù)來(lái)更新當(dāng)前時(shí)刻四元數(shù)，必然會(huì)導(dǎo)致姿態(tài)切換后，姿態(tài)角的紊亂。 因?yàn)樵谧藨B(tài)切換后，當(dāng)前時(shí)刻四元數(shù)依舊利用前一時(shí)刻四元數(shù)來(lái)更新，最終導(dǎo)致軌跡的錯(cuò) 誤。
[0014] 所述上述姿態(tài)切換后，四元數(shù)的更新技術(shù)方案如下： 姿態(tài)切換穩(wěn)定后，首先進(jìn)行坐標(biāo)變換，然后利用姿態(tài)切換穩(wěn)定后的第一點(diǎn)加速度計(jì)和 磁強(qiáng)計(jì)數(shù)據(jù)來(lái)重新計(jì)算姿態(tài)角，再將姿態(tài)角轉(zhuǎn)換成四元數(shù)，后期姿態(tài)角更新同上所述。
[0015] 所述姿態(tài)切換前后固定姿態(tài)角差為不同姿態(tài)情況下，便攜式手持終端所處的位置 不一樣，這樣就會(huì)導(dǎo)致姿態(tài)切換后的軌跡與姿態(tài)切換的軌跡存在一個(gè)固定姿態(tài)角差； 所述固定姿態(tài)角差解決技術(shù)方案如下： 用姿態(tài)切換后穩(wěn)定第一點(diǎn)的姿態(tài)角角減去姿態(tài)切換前穩(wěn)定最后一點(diǎn)姿態(tài)角，得到固定 姿態(tài)角差。姿態(tài)切換后，更新的姿態(tài)角都要減去這個(gè)固定姿態(tài)角差。
[0016] 在姿態(tài)切換過(guò)程中根據(jù)前一時(shí)刻四元數(shù)和陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì)解算出的 姿態(tài)角信息是完全紊亂的。
【權(quán)利要求】
1. 全姿態(tài)環(huán)境下自主室內(nèi)定位算法，其特征在于：包括行人在定位過(guò)程中便攜式手持 終端可能出現(xiàn)的姿態(tài)判別算法、姿態(tài)切換過(guò)程跟蹤算法、姿態(tài)切換后的姿態(tài)分類算法、姿態(tài) 切換過(guò)程中姿態(tài)角精度優(yōu)化算法和姿態(tài)切換后的定位算法更新準(zhǔn)則等； 所述全姿態(tài)指，便攜式設(shè)備在用戶手中可能出現(xiàn)的各種姿態(tài)以及各種姿態(tài)之間的切 換； 所述行人在定位過(guò)程中便攜式手持終端可能出現(xiàn)的姿態(tài)判別算法包括手持姿態(tài)算法、 耳邊姿態(tài)算法、腰間姿態(tài)算法； 所述手持姿態(tài)指，將便攜式手持終端水平放在右手或左手上； 所述打電話姿態(tài)指，將便攜式手持終端放在右耳或左耳旁； 所述腰間姿態(tài)指，將便攜式手持終端放在右腰間兜或左腰間兜里； 所述姿態(tài)切換過(guò)程跟蹤算法包括對(duì)以下姿態(tài)切換方式的跟蹤： 手持(左）一耳邊(左)、手持(左）一耳邊(右)、手持(左）一腰間（左)、手持(左）一腰間 (右)、手持(右）一耳邊(左)、手持(右）一耳邊(右)、手持(右）一腰間(左)、手持(右）一腰間 (右)、耳邊(左）一手持(左)、耳邊(左）一手持(右)、耳邊(左）一腰間（左)、耳邊(左一腰間 (右)、耳邊(右）一手持(左)、耳邊(右）一手持(右)、耳邊(右）一腰間（左)、耳邊(右）一腰間 (右)、腰間(左）一手持(左)、腰間(左）一手持(右)、腰間(左）一耳邊(左)、腰間(左）一耳邊 (右)、腰間（右）一手持(左)、腰間（右）一手持(右)、腰間（右）一耳邊(左)、腰間（右）一耳邊 (右）； 所述姿態(tài)切換后的姿態(tài)分類為手持(左)、手持(右)、耳邊(左)、耳邊(右)、腰間(左)和腰 間(右）6種姿態(tài)切換方式； 所述姿態(tài)切換后的定位算法更新準(zhǔn)則指確定了姿態(tài)切換方式后，更新到對(duì)應(yīng)姿態(tài)切換 方式下定位算法。
2. 根據(jù)權(quán)利要求1所述的全姿態(tài)環(huán)境下自主室內(nèi)定位算法，其特征在于：所述各種姿 態(tài)下定位算法的思想指不同姿態(tài)切換下坐標(biāo)變換的確定，即旋轉(zhuǎn)矩陣的確定。
3. 所述坐標(biāo)變換指，不同姿態(tài)下MEMS慣性傳感器數(shù)據(jù)在載體坐標(biāo)系中是不同的，這時(shí) 需要重新將傳感器的數(shù)據(jù)變換到姿態(tài)切換后的載體坐標(biāo)系下。
4. 所述旋轉(zhuǎn)矩陣指，不同姿態(tài)對(duì)應(yīng)的載體坐標(biāo)系之間轉(zhuǎn)化的度量。
5. 根據(jù)權(quán)利要求1所述的全姿態(tài)環(huán)境下自主室內(nèi)定位算法，其特征在于：所述姿態(tài)切 換過(guò)程跟蹤精確度取決于切換條件的選取，所選切換條件要能將姿態(tài)切換與同一姿態(tài)下正 常轉(zhuǎn)彎進(jìn)行區(qū)別。
6. 根據(jù)權(quán)利要求1所述的全姿態(tài)環(huán)境下自主室內(nèi)定位算法，其特征在于：所述姿態(tài)切 換后對(duì)姿態(tài)分類精確度取決于切換條件的選取，所選切換條件要能將姿態(tài)切換與同一姿態(tài) 下正常轉(zhuǎn)彎進(jìn)行區(qū)別。
7. 根據(jù)權(quán)利要求1所述的全姿態(tài)環(huán)境下自主室內(nèi)定位算法，其特征在于：所述姿態(tài)切 換后定位算法更新是指首先確定姿態(tài)切換的方式，再將對(duì)應(yīng)切換方式下定位算法切換為當(dāng) 前定位算法。
8. 根據(jù)權(quán)利要求1所述的全姿態(tài)環(huán)境下自主室內(nèi)定位算法，其特征在于：所述姿態(tài)切 換過(guò)程中姿態(tài)角精度優(yōu)化是指，在姿態(tài)切換過(guò)程中由于陀螺儀和、速度計(jì)和磁強(qiáng)計(jì)的數(shù)據(jù) 完全不可信，本發(fā)明采用姿態(tài)切換前姿態(tài)角信息對(duì)姿態(tài)切換過(guò)程中的姿態(tài)角進(jìn)行優(yōu)化處
【文檔編號(hào)】G01C21/16GK104154913SQ201410297278
【公開(kāi)日】2014年11月19日 申請(qǐng)日期:2014年6月28日 優(yōu)先權(quán)日:2014年6月28日
【發(fā)明者】黎蕾蕾, 王仕亮, 劉宇, 曹陽(yáng), 黃河明, 陳燕蘋, 史莉莉, 江宏毅, 王伊冰, 王瑞杰 申請(qǐng)人:南京諾導(dǎo)電子科技有限公司