本發(fā)明屬于室內(nèi)人員定位，具體涉及一種基于因子圖的gnss/pdr無縫定位方法和裝置，應(yīng)用于行人導(dǎo)航、可穿戴設(shè)備定位、智慧安防、健康監(jiān)測(cè)及應(yīng)急救援等領(lǐng)域。
背景技術(shù)：
1、隨著可穿戴設(shè)備與多模態(tài)傳感器技術(shù)的發(fā)展，基于智能手表、手環(huán)等輕量化終端實(shí)現(xiàn)對(duì)行人運(yùn)動(dòng)狀態(tài)與位置變化的連續(xù)感知已成為研究熱點(diǎn)。可穿戴設(shè)備通常集成加速度計(jì)、陀螺儀、磁力計(jì)及gnss模塊，具有便攜性強(qiáng)、能耗低的特點(diǎn)，可在無需額外基礎(chǔ)設(shè)施的條件下實(shí)現(xiàn)軌跡重建與行為監(jiān)測(cè)。
2、基于全球?qū)Ш叫l(wèi)星系統(tǒng)（gnss）的戶外定位技術(shù)在開闊環(huán)境中可提供較高精度，但在城市峽谷、林蔭道路、地鐵入口等復(fù)雜環(huán)境中，gnss信號(hào)易受到非視距傳播（nlos）和多路徑干擾而顯著退化，導(dǎo)致定位點(diǎn)跳變、漂移或短時(shí)不可用。在室內(nèi)或地下空間中，gnss基本無法工作，難以滿足連續(xù)行人定位需求。
3、為彌補(bǔ)gnss不穩(wěn)定的缺陷，行人航位推算（pdr）利用內(nèi)置imu估計(jì)步態(tài)事件、步長(zhǎng)及航向變化，可在短時(shí)間內(nèi)提供平滑的相對(duì)位移推算結(jié)果。然而，由于pdr本質(zhì)上對(duì)噪聲做積分運(yùn)算，誤差會(huì)隨時(shí)間累積；尤其在腕帶式imu場(chǎng)景中，擺臂運(yùn)動(dòng)造成的周期振蕩、磁場(chǎng)擾動(dòng)導(dǎo)致的航向跳變、陀螺積分漂移等因素均使長(zhǎng)期軌跡出現(xiàn)顯著偏移，難以獨(dú)立提供可靠絕對(duì)定位。
4、現(xiàn)有技術(shù)中，研究者嘗試通過ekf、ukf、粒子濾波或固定權(quán)重加權(quán)融合方法結(jié)合gnss與pdr，但濾波模型多基于線性化或固定噪聲假設(shè)，難以根據(jù)gnss信號(hào)質(zhì)量退化程度動(dòng)態(tài)調(diào)整觀測(cè)可信度。當(dāng)gnss出現(xiàn)遮擋、星歷變化或噪聲突增時(shí)，濾波方法容易受到異常觀測(cè)影響而快速失穩(wěn)；而僅依賴pdr會(huì)導(dǎo)致誤差不斷積累，無法保證軌跡的長(zhǎng)期一致性。
5、因子圖優(yōu)化（factorgraphoptimization,fgo）作為近年來發(fā)展迅速的軌跡估計(jì)框架，能夠以圖結(jié)構(gòu)統(tǒng)一表達(dá)多源約束，通過全局優(yōu)化同時(shí)利用歷史與當(dāng)前觀測(cè)獲得狀態(tài)的聯(lián)合最優(yōu)解。相比傳統(tǒng)濾波方法，因子圖在噪聲建模、權(quán)重自適應(yīng)和約束表達(dá)方面更加靈活，尤其適用于gnss質(zhì)量變化顯著的場(chǎng)景，因此成為gnss/pdr融合的重要方向。
6、然而，已有因子圖方法多針對(duì)足部imu或剛性安裝的慣導(dǎo)設(shè)備設(shè)計(jì)，并未考慮腕帶式imu特有的噪聲模式、擺臂干擾與航向不穩(wěn)定問題。腕帶加速度信號(hào)受手臂擺動(dòng)影響顯著，線性加速度分離難度更高；磁力計(jì)失準(zhǔn)現(xiàn)象頻繁發(fā)生，使航向角易出現(xiàn)突跳；步態(tài)檢測(cè)亦容易受到偽峰干擾而引發(fā)誤檢或漏檢。若直接將傳統(tǒng)gnss/pdr融合框架應(yīng)用于腕帶設(shè)備，融合結(jié)果往往不穩(wěn)定且恢復(fù)速度較慢。
技術(shù)實(shí)現(xiàn)思路
1、為解決現(xiàn)有技術(shù)存在的問題，本發(fā)明提供一種基于因子圖的gnss/pdr無縫定位方法和裝置。
2、為實(shí)現(xiàn)上述目的，本發(fā)明提供了如下方案：
3、一種基于因子圖的gnss/pdr無縫定位方法，包括：
4、步驟s1、獲取腕帶可穿戴設(shè)備的加速度、角速度、磁場(chǎng)強(qiáng)度和定位觀測(cè)數(shù)據(jù)；
5、步驟s2、根據(jù)加速度，提取線性加速度；
6、步驟s3、根據(jù)線性加速度進(jìn)行步態(tài)檢測(cè)并估計(jì)步長(zhǎng)；
7、步驟s4、根據(jù)三軸加速度和磁場(chǎng)強(qiáng)度，基于姿態(tài)補(bǔ)償?shù)玫胶较蚪牵?/p>
8、步驟s5、根據(jù)步長(zhǎng)和航向角，進(jìn)行腕帶式pdr推算得到連續(xù)相對(duì)軌跡；
9、步驟s6、根據(jù)連續(xù)相對(duì)軌跡以及和定位觀測(cè)數(shù)據(jù)構(gòu)建因子圖，并對(duì)狀態(tài)序列進(jìn)行非線性優(yōu)化，得到室內(nèi)外環(huán)境下連續(xù)穩(wěn)定的融合定位軌跡。
10、作為優(yōu)選，步驟s2中，對(duì)三軸加速度進(jìn)行重力分離處理，得到線性加速度序列。
11、作為優(yōu)選，步驟s3中，基于線性加速度構(gòu)建適用于腕部強(qiáng)噪聲環(huán)境的多尺度差分量，并根據(jù)設(shè)定的步態(tài)判斷準(zhǔn)則識(shí)別步態(tài)事件并確定步長(zhǎng)。
12、作為優(yōu)選，步驟s6中，構(gòu)建包含先驗(yàn)因子、pdr里程計(jì)因子及gnss位置因子的因子圖，并通過非線性最小二乘優(yōu)化求解全局狀態(tài)量，獲得融合后的連續(xù)定位結(jié)果。
13、本發(fā)明還提供一種基于因子圖的gnss/pdr無縫定位裝置，包括：
14、第一處理模塊，用于獲取腕帶可穿戴設(shè)備的加速度、角速度、磁場(chǎng)強(qiáng)度和定位觀測(cè)數(shù)據(jù)；
15、第二處理模塊，用于根據(jù)加速度，提取線性加速度；
16、第三處理模塊，用于根據(jù)線性加速度進(jìn)行步態(tài)檢測(cè)并估計(jì)步長(zhǎng)；
17、第四處理模塊，用于根據(jù)三軸加速度和磁場(chǎng)強(qiáng)度，基于姿態(tài)補(bǔ)償?shù)玫胶较蚪牵?/p>
18、第五處理模塊，用于根據(jù)步長(zhǎng)和航向角，進(jìn)行腕帶式pdr推算得到連續(xù)相對(duì)軌跡；
19、第六處理模塊，用于根據(jù)連續(xù)相對(duì)軌跡以及定位觀測(cè)數(shù)據(jù)構(gòu)建因子圖，并對(duì)狀態(tài)序列進(jìn)行非線性優(yōu)化，得到室內(nèi)外環(huán)境下連續(xù)穩(wěn)定的融合定位軌跡。
20、作為優(yōu)選，第二處理模塊對(duì)三軸加速度進(jìn)行重力分離處理，得到線性加速度序列。
21、作為優(yōu)選，第三處理模塊基于線性加速度構(gòu)建適用于腕部強(qiáng)噪聲環(huán)境的多尺度差分量，并根據(jù)設(shè)定的步態(tài)判斷準(zhǔn)則識(shí)別步態(tài)事件并確定步長(zhǎng)。
22、作為優(yōu)選，第六處理模塊構(gòu)建包含先驗(yàn)因子、pdr里程計(jì)因子及gnss位置因子的因子圖，并通過非線性最小二乘優(yōu)化求解全局狀態(tài)量，獲得融合后的連續(xù)定位結(jié)果。
23、與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果為：
24、1、本發(fā)明通過在腕帶imu信號(hào)中引入線性加速度分解、多尺度差分步態(tài)檢測(cè)與姿態(tài)補(bǔ)償航向估計(jì)，使腕部慣性數(shù)據(jù)在擺臂干擾較強(qiáng)、噪聲較大的情況下仍能夠獲得較為穩(wěn)定的步態(tài)觸發(fā)與方向估計(jì)。在此基礎(chǔ)上，通過構(gòu)建包含先驗(yàn)因子、pdr里程計(jì)因子與gnss位置因子的統(tǒng)一因子圖模型，本發(fā)明能夠在gnss觀測(cè)可靠性變化的條件下實(shí)現(xiàn)多源約束的聯(lián)合優(yōu)化，有效減少gnss異常跳變與pdr累積誤差對(duì)軌跡連續(xù)性造成的影響。
25、2、在開闊場(chǎng)地中，本發(fā)明能夠充分利用gnss的絕對(duì)定位約束，使推算軌跡保持良好的一致性；在室內(nèi)外過渡與衛(wèi)星信號(hào)衰減區(qū)段，通過pdr提供相對(duì)位移信息維持軌跡連續(xù)性，并在gnss恢復(fù)后借助因子圖的全局優(yōu)化框架抑制前期誤差累積，使軌跡形態(tài)更接近真實(shí)路徑。本發(fā)明可有效抑制pdr的長(zhǎng)期累積誤差，并在gnss恢復(fù)后通過因子圖優(yōu)化實(shí)現(xiàn)軌跡整體糾正，提升定位魯棒性。
26、3、本發(fā)明適用于在通用處理器或嵌入式設(shè)備上進(jìn)行離線推算與融合處理；所構(gòu)建的因子圖框架具有良好的可擴(kuò)展性，可進(jìn)一步引入氣壓計(jì)、藍(lán)牙或視覺傳感信息以增強(qiáng)系統(tǒng)魯棒性。本發(fā)明可為可穿戴定位、健康監(jiān)測(cè)、行為識(shí)別及應(yīng)急輔助等應(yīng)用提供基礎(chǔ)技術(shù)支持，具有良好的工程應(yīng)用價(jià)值和推廣潛力。
1.一種基于因子圖的gnss/pdr無縫定位方法，其特征在于，包括：
2.如權(quán)利要求1所述的基于因子圖的gnss/pdr無縫定位方法，其特征在于，步驟s2中，對(duì)三軸加速度進(jìn)行重力分離處理，得到線性加速度序列。
3.如權(quán)利要求2所述的基于因子圖的gnss/pdr無縫定位方法，其特征在于，步驟s3中，基于線性加速度構(gòu)建適用于腕部強(qiáng)噪聲環(huán)境的多尺度差分量，并根據(jù)設(shè)定的步態(tài)判斷準(zhǔn)則識(shí)別步態(tài)事件并確定步長(zhǎng)。
4.如權(quán)利要求3所述的基于因子圖的gnss/pdr無縫定位方法，其特征在于，步驟s6中，構(gòu)建包含先驗(yàn)因子、pdr里程計(jì)因子及gnss位置因子的因子圖，并通過非線性最小二乘優(yōu)化求解全局狀態(tài)量，獲得融合后的連續(xù)定位結(jié)果。
5.一種基于因子圖的gnss/pdr無縫定位裝置，其特征在于，包括：
6.如權(quán)利要求5所述的基于因子圖的gnss/pdr無縫定位裝置，其特征在于，第二處理模塊對(duì)三軸加速度進(jìn)行重力分離處理，得到線性加速度序列。
7.如權(quán)利要求6所述的基于因子圖的gnss/pdr無縫定位裝置，其特征在于，第三處理模塊基于線性加速度構(gòu)建適用于腕部強(qiáng)噪聲環(huán)境的多尺度差分量，并根據(jù)設(shè)定的步態(tài)判斷準(zhǔn)則識(shí)別步態(tài)事件并確定步長(zhǎng)。
8.如權(quán)利要求7所述的基于因子圖的gnss/pdr無縫定位裝置，其特征在于，第六處理模塊構(gòu)建包含先驗(yàn)因子、pdr里程計(jì)因子及gnss位置因子的因子圖，并通過非線性最小二乘優(yōu)化求解全局狀態(tài)量，獲得融合后的連續(xù)定位結(jié)果。